Camera and Control of UR5

edit

Hi all. I am using Ros1 Kinetic on Ubuntu 16.04LTS, gazebo7 and rviz 1.12.17, I am simulating Universal robot UR5 but there are problems one of them is the robot is shakey and 2nd is I cant controll the robot I want to simulate and add a camera to it.

the following is my launch file:

<?xml version="1.0"?>
<launch>
  <arg name="gui" default="True" />

  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find ur5_description)/urdf/ur5_robotiq85_gripper.urdf.xacro" />

  <!-- Adding camera in rviz -->
  <group ns="camera1">
    <node pkg="tf" type="static_transform_publisher" name="camera_broadcaster"
      args="-15 0 15 1.57 3.14 1.1 map camera1 10" />
    <node name="camera_info" pkg="rostopic" type="rostopic"
      args="pub camera_info sensor_msgs/CameraInfo
     '{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: 'camera1'},
      height: 480, width: 640, distortion_model: 'plumb_bob',
      D: [0],
      K: [500.0, 0.0, 320, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0],
      R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
      P: [500.0, 0.0, 320, 0.0, 0.0, 500, 240, 0.0, 0.0, 0.0, 1.0, 0.0],
      binning_x: 0, binning_y: 0,
      roi: {x_offset: 0, y_offset: 0, height: 480, width: 640, do_rectify: false}}' -r 2"
      output="screen"/>
  </group>

<!--Gazebo empty world launch file-->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="debug" value="false" />
        <arg name="gui" value="true" />
        <arg name="paused" value="false"/>
        <arg name="use_sim_time" value="false"/>
        <arg name="headless" value="false"/>
        <arg name="verbose" value="true"/>
  </include>

  <!-- Start your own node for the joint_state_publisher.py in scripts folder -->

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>

  <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model UR5 -verbose" output="screen"/>

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find ur5_description)/config/ur5.rviz" required="true" />


</launch>

and this is my ur5_robotiq85_gripper.urdf.xacro file

<?xml version="1.0"?>
<robot name="ur5" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:include filename="$(find ur5_description)/urdf/ur5_joint_limited_robot.urdf.xacro" />
  <xacro:include filename="$(find robotiq_description)/urdf/robotiq_85_gripper.urdf.xacro" />
  <xacro:property name="camera_link" value="0.05" /> <!-- Size of square 'camera' box -->
  <xacro:property name="height3" value="0.05" /> <!-- height3 - axel_offset*2 -->
  <xacro:property name="axel_offset" value="0.05" /> <!-- height3 - axel_offset*2 -->


  <!-- Robotiq Coupler -->
  <!--  + Height added by the coupler: 8mm -->
  <!--  + Reference frame: at the middle (4mm) -->
  <link name="robotiq_coupler">
    <visual>
      <geometry>
        <mesh filename="package://robotiq_description/meshes/robotiq_85_coupler.stl" />
      </geometry>
      <material name="flat_black"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://robotiq_description/meshes/robotiq_85_coupler.stl" />
      </geometry>
    </collision>
    <inertial>
      <origin xyz="2.073e-05   1.45286e-03  -1.1049e-03" rpy="0 ...