Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Kinectv2 not working with real ur5 but does in simulation

Hi everyone,

I am using the UR5 with a own designed gripper as end-effector and KinectV2 to 'map' the environment so that the UR5 will avoid collisions with a human it might be working with. I have run some tests: - Simulated UR5 and simulated Kinect in gazebo. I can see the 'octomap' getting added to the planning scene in moveit.rviz and the UR5 avoids collisions with object placed in Gazebo. - Simulated UR5 in Gazebo and real Kinect, same as above. - Real UR5 and real Kinect. I cannot see the planning scene being added in moveit.rviz.

I get a warning from moveit that says: Where driver_arm_joint is the joint that controls the gripper state.

My suggestion is that moveit cannot use the planning scene it is creating because the complete state of the robot is not yet known.

My question is: Is there a way to initialize the endeffector group such that you define a start state? Currently this is the content of the srdf: <robot name="ur5"> <group name="manipulator"> <chain base_link="base_link" tip_link="ee_link"/> </group>

<group name="endeffector">
    <link name="mount_link"/>
    <link name="plate_link"/>
    <link name="driver_arm_link"/>
    <link name="driven_arm_link"/>
    <link name="left_arm_front_link"/>
    <link name="left_arm_back_link"/>
    <link name="right_arm_front_link"/>
    <link name="right_arm_back_link"/>
    <link name="left_finger_link"/>
    <link name="right_finger_link"/>
    <link name="gripper_ee_link"/>
</group>

<group_start_state name="start_gripper" group="endeffector">
    <joint name="driver_arm_joint" value="0"/>
</group_start_state>
<!-- Home state for manipulator group -->
<group_state name="home" group="manipulator">
    <joint name="elbow_joint" value="0" />
    <joint name="shoulder_lift_joint" value="0" />
    <joint name="shoulder_pan_joint" value="0" />
    <joint name="wrist_1_joint" value="0" />
    <joint name="wrist_2_joint" value="0" />
    <joint name="wrist_3_joint" value="0" />
</group_state>

<!-- Up state for manipulator group -->
<group_state name="up" group="manipulator">
    <joint name="elbow_joint" value="0" />
    <joint name="shoulder_lift_joint" value="-1.5707" />
    <joint name="shoulder_pan_joint" value="0" />
    <joint name="wrist_1_joint" value="-1.5707" />
    <joint name="wrist_2_joint" value="0" />
    <joint name="wrist_3_joint" value="0" />
</group_state>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="moveit_ee" parent_link="ee_link" group="endeffector" parent_group="manipulator"/>
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)
<virtual_joint name="fixed_base" type="fixed" parent_frame="world" child_link="base_link" />

--> <disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent"/> <disable_collisions link1="ee_link" link2="wrist_1_link" reason="Never"/> <disable_collisions link1="ee_link" link2="wrist_2_link" reason="Never"/> <disable_collisions link1="ee_link" link2="wrist_3_link" reason="Adjacent"/> <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent"/> <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent"/> <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent"/> <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent"/> <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never"/> <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent"/> <disable_collisions link1="gripper_ee_link" link2="shoulder_link" reason="Never"/> <disable_collisions link1="gripper_ee_link" link2="forearm_link" reason="Never"/> <disable_collisions link1="gripper_ee_link" link2="upper_arm_link" reason="Never"/> <disable_collisions link1="gripper_ee_link" link2="wrist_1_link" reason="Never"/> <disable_collisions link1="gripper_ee_link" link2="wrist_2_link" reason="Never"/> <disable_collisions link1="gripper_ee_link" link2="wrist_3_link" reason="Never"/> <disable_collisions link1="ee_link" link2="mount_link" reason="Adjacent"/> <disable_collisions link1="wrist_3_link" link2="mount_link" reason="Adjacent"/> <disable_collisions link1="mount_link" link2="plate_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="driver_arm_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="driven_arm_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="left_arm_front_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="left_arm_back_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="right_arm_front_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="right_arm_back_link" reason="Adjacent"/> <disable_collisions link1="driver_arm_link" link2="driven_arm_link" reason="Adjacent"/> <disable_collisions link1="left_arm_front_link" link2="left_finger_link" reason="Adjacent"/> <disable_collisions link1="left_arm_back_link" link2="left_finger_link" reason="Adjacent"/> <disable_collisions link1="right_arm_front_link" link2="right_finger_link" reason="Adjacent"/> <disable_collisions link1="right_arm_back_link" link2="right_finger_link" reason="Adjacent"/> </robot>

Kinectv2 not working with real ur5 but does in simulation

Hi everyone,

I am using the UR5 with a own designed gripper as end-effector and KinectV2 to 'map' the environment so that the UR5 will avoid collisions with a human it might be working with. I have run some tests: - Simulated UR5 and simulated Kinect in gazebo. I can see the 'octomap' getting added to the planning scene in moveit.rviz and the UR5 avoids collisions with object placed in Gazebo. - Simulated UR5 in Gazebo and real Kinect, same as above. - Real UR5 and real Kinect. I cannot see the planning scene being added in moveit.rviz.

I get a warning from moveit that says: Where driver_arm_joint is the joint that controls the gripper state.

My suggestion is that moveit cannot use the planning scene it is creating because the complete state of the robot is not yet known.

UPDATE: My gripper also becomes uncontrollable when launching the real UR5 and Kinectv2.

My question is: Is there a way to initialize the endeffector group such that you define a start state? Currently this is the content of the srdf: srdf:

<robot name="ur5"> <group name="manipulator"> <chain base_link="base_link" tip_link="ee_link"/> </group>

<group name="endeffector">
    <link name="mount_link"/>
    <link name="plate_link"/>
    <link name="driver_arm_link"/>
    <link name="driven_arm_link"/>
    <link name="left_arm_front_link"/>
    <link name="left_arm_back_link"/>
    <link name="right_arm_front_link"/>
    <link name="right_arm_back_link"/>
    <link name="left_finger_link"/>
    <link name="right_finger_link"/>
    <link name="gripper_ee_link"/>
</group>

<group_start_state name="start_gripper" group="endeffector">
    <joint name="driver_arm_joint" value="0"/>
</group_start_state>
<!-- Home state for manipulator group -->
<group_state name="home" group="manipulator">
    <joint name="elbow_joint" value="0" />
    <joint name="shoulder_lift_joint" value="0" />
    <joint name="shoulder_pan_joint" value="0" />
    <joint name="wrist_1_joint" value="0" />
    <joint name="wrist_2_joint" value="0" />
    <joint name="wrist_3_joint" value="0" />
</group_state>

<!-- Up state for manipulator group -->
<group_state name="up" group="manipulator">
    <joint name="elbow_joint" value="0" />
    <joint name="shoulder_lift_joint" value="-1.5707" />
    <joint name="shoulder_pan_joint" value="0" />
    <joint name="wrist_1_joint" value="-1.5707" />
    <joint name="wrist_2_joint" value="0" />
    <joint name="wrist_3_joint" value="0" />
</group_state>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="moveit_ee" parent_link="ee_link" group="endeffector" parent_group="manipulator"/>
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)
<virtual_joint name="fixed_base" type="fixed" parent_frame="world" child_link="base_link" />

--> <disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent"/> <disable_collisions link1="ee_link" link2="wrist_1_link" reason="Never"/> <disable_collisions link1="ee_link" link2="wrist_2_link" reason="Never"/> <disable_collisions link1="ee_link" link2="wrist_3_link" reason="Adjacent"/> <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent"/> <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent"/> <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent"/> <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent"/> <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never"/> <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent"/> <disable_collisions link1="gripper_ee_link" link2="shoulder_link" reason="Never"/> <disable_collisions link1="gripper_ee_link" link2="forearm_link" reason="Never"/> <disable_collisions link1="gripper_ee_link" link2="upper_arm_link" reason="Never"/> <disable_collisions link1="gripper_ee_link" link2="wrist_1_link" reason="Never"/> <disable_collisions link1="gripper_ee_link" link2="wrist_2_link" reason="Never"/> <disable_collisions link1="gripper_ee_link" link2="wrist_3_link" reason="Never"/> <disable_collisions link1="ee_link" link2="mount_link" reason="Adjacent"/> <disable_collisions link1="wrist_3_link" link2="mount_link" reason="Adjacent"/> <disable_collisions link1="mount_link" link2="plate_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="driver_arm_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="driven_arm_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="left_arm_front_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="left_arm_back_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="right_arm_front_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="right_arm_back_link" reason="Adjacent"/> <disable_collisions link1="driver_arm_link" link2="driven_arm_link" reason="Adjacent"/> <disable_collisions link1="left_arm_front_link" link2="left_finger_link" reason="Adjacent"/> <disable_collisions link1="left_arm_back_link" link2="left_finger_link" reason="Adjacent"/> <disable_collisions link1="right_arm_front_link" link2="right_finger_link" reason="Adjacent"/> <disable_collisions link1="right_arm_back_link" link2="right_finger_link" reason="Adjacent"/> </robot>

Kinectv2 not working with real ur5 but does in simulation

Hi everyone,

I am using the UR5 with a own designed gripper as end-effector and KinectV2 to 'map' the environment so that the UR5 will avoid collisions with a human it might be working with. I have run some tests: - tests:

  • Simulated UR5 and simulated Kinect in gazebo. I can see the 'octomap' getting added to the planning scene in moveit.rviz and the UR5 avoids collisions with object placed in Gazebo. - Gazebo.
  • Simulated UR5 in Gazebo and real Kinect, same as above. - above.
  • Real UR5 and real Kinect. I cannot see the planning scene being added in moveit.rviz.

I get a warning from moveit that says: says:

[ WARN] [1516619887.119811534]: The complete state of the robot is not yet known.  Missing driver_arm_joint

Where driver_arm_joint driver_arm_joint is the joint that controls the gripper state.

My suggestion is that moveit cannot use the planning scene it is creating because the complete state of the robot is not yet known.

UPDATE: My gripper also becomes uncontrollable when launching the real UR5 and Kinectv2.

My question is: Is there a way to initialize the endeffector group such that you define a start state? Currently this is the content of the srdf:

<?xml version="1.0" ?>
<!--This does not replace URDF, and is not an extension of URDF.
    This is a format for representing semantic information about the robot structure.
    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="ur5">
     <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
    <group name="manipulator">
        <chain base_link="base_link" tip_link="ee_link"/>
    </group>

tip_link="ee_link" />
    </group>

    <group name="endeffector">
     <link name="mount_link"/>
     <link name="plate_link"/>
     <link name="driver_arm_link"/>
     <link name="driven_arm_link"/>
     <link name="left_arm_front_link"/>
     <link name="left_arm_back_link"/>
     <link name="right_arm_front_link"/>
     <link name="right_arm_back_link"/>
     <link name="left_finger_link"/>
     <link name="right_finger_link"/>
     <link name="gripper_ee_link"/>
 </group>

 <group_start_state name="start_gripper" group="endeffector">
     <joint name="driver_arm_joint" value="0"/>
 </group_start_state>
 <!-- Home state for manipulator group -->
 <group_state name="home" group="manipulator">
     <joint name="elbow_joint" value="0" />
     <joint name="shoulder_lift_joint" value="0" />
     <joint name="shoulder_pan_joint" value="0" />
     <joint name="wrist_1_joint" value="0" />
     <joint name="wrist_2_joint" value="0" />
     <joint name="wrist_3_joint" value="0" />
 </group_state>

 <!-- Up state for manipulator group -->
 <group_state name="up" group="manipulator">
     <joint name="elbow_joint" value="0" />
     <joint name="shoulder_lift_joint" value="-1.5707" />
     <joint name="shoulder_pan_joint" value="0" />
     <joint name="wrist_1_joint" value="-1.5707" />
     <joint name="wrist_2_joint" value="0" />
     <joint name="wrist_3_joint" value="0" />
 </group_state>
 <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
 <end_effector name="moveit_ee" parent_link="ee_link" group="endeffector" parent_group="manipulator"/>
 <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)
 <virtual_joint name="fixed_base" type="fixed" parent_frame="world" child_link="base_link" />

--> <!--DISABLE COLLISIONS: Within the robot--> <disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent"/> reason="Adjacent" /> <disable_collisions link1="ee_link" link2="wrist_1_link" reason="Never"/> reason="Never" /> <disable_collisions link1="ee_link" link2="wrist_2_link" reason="Never"/> reason="Never" /> <disable_collisions link1="ee_link" link2="wrist_3_link" reason="Adjacent"/> reason="Adjacent" /> <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent"/> reason="Adjacent" /> <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent"/> reason="Adjacent" /> <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent"/> reason="Adjacent" /> <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent"/> reason="Adjacent" /> <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never"/> reason="Never" /> <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent"/> reason="Adjacent" /> <disable_collisions link1="gripper_ee_link" link2="shoulder_link" reason="Never"/> <disable_collisions link1="gripper_ee_link" link2="forearm_link" reason="Never"/> <disable_collisions link1="gripper_ee_link" link2="upper_arm_link" reason="Never"/> <disable_collisions link1="gripper_ee_link" link2="wrist_1_link" reason="Never"/> <disable_collisions link1="gripper_ee_link" link2="wrist_2_link" reason="Never"/> <disable_collisions link1="gripper_ee_link" link2="wrist_3_link" reason="Never"/> <!-- DISABLE COLLISIONS: Within the end-effector --> <disable_collisions link1="ee_link" link2="mount_link" reason="Adjacent"/> <disable_collisions link1="wrist_3_link" link1 ="wrist_3_link" link2="mount_link" reason="Adjacent"/> <disable_collisions link1="mount_link" link2="plate_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="driver_arm_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="driven_arm_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="left_arm_front_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="left_arm_back_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="right_arm_front_link" reason="Adjacent"/> <disable_collisions link1="plate_link" link2="right_arm_back_link" reason="Adjacent"/> <disable_collisions link1="driver_arm_link" link2="driven_arm_link" reason="Adjacent"/> <disable_collisions link1="left_arm_front_link" link2="left_finger_link" reason="Adjacent"/> <disable_collisions link1="left_arm_back_link" link2="left_finger_link" reason="Adjacent"/> <disable_collisions link1="right_arm_front_link" link2="right_finger_link" reason="Adjacent"/> <disable_collisions link1="right_arm_back_link" link2="right_finger_link" reason="Adjacent"/> </robot>

</robot>

Kinectv2 not working with real ur5 but does in simulation

Hi everyone,

I am using the UR5 with a own designed gripper as end-effector and KinectV2 to 'map' the environment so that the UR5 will avoid collisions with a human it might be working with. I have run some tests:

  • Simulated UR5 and simulated Kinect in gazebo. I can see the 'octomap' getting added to the planning scene in moveit.rviz and the UR5 avoids collisions with object placed in Gazebo.
  • Simulated UR5 in Gazebo and real Kinect, same as above.
  • Real UR5 and real Kinect. I cannot see the planning scene being added in moveit.rviz.

I get a warning from moveit that says:

[ WARN] [1516619887.119811534]: The complete state of the robot is not yet known.  Missing driver_arm_joint

Where driver_arm_joint is the joint that controls the gripper state.

My suggestion is that moveit cannot use the planning scene it is creating because the complete state of the robot is not yet known.

UPDATE: My gripper also becomes uncontrollable when launching the real UR5 and Kinectv2.

FINAL UPDATE: First I remapped the topic /joint_states (published by ur_modern_driver) to /joint_states_ur5. Then I wrote a node that subscribes to the /joint_states_ur5 topic and add a sensors_msgs/JointState message to the tuple of the gotten published by the ur_driver. Then I republish the message under the topic /joint_states. The python node is shown below.

import math
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Bool


class ur5_republisher():
    def __init__(self):
        rospy.init_node('Cmd_Scaler')
        self.pub = rospy.Publisher('/joint_states', JointState, queue_size=10)
        rospy.Subscriber("/joint_states_ur5", JointState, self.joint_state_callback)

        self.name = 'driver_arm_joint'
        self.position = 0
        self.velocity = 0
        self.effort = 0

    def appendT(self,t,x):
    l = list(t)
    l.append(x)
    return tuple(l)

    def joint_state_callback(self, data):
        data.name.append(self.name)
    data.position = self.appendT(data.position, self.position)
    data.velocity = self.appendT(data.velocity, self.position)
    data.effort = self.appendT(data.effort, self.position)
       self.pub.publish(data)

if __name__ == '__main__':
    ur5_r = ur5_republisher()
    while not rospy.is_shutdown():
    # self.cycle()
    rospy.spin()

My question is: Is there a way to initialize the endeffector group such that you define a start state? Currently this is the content of the srdf:

<?xml version="1.0" ?>
<!--This does not replace URDF, and is not an extension of URDF.
    This is a format for representing semantic information about the robot structure.
    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="ur5">
    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
    <group name="manipulator">
        <chain base_link="base_link" tip_link="ee_link" />
    </group>

    <group name="endeffector">
        <link name="mount_link"/>
        <link name="plate_link"/>
        <link name="driver_arm_link"/>
        <link name="driven_arm_link"/>
        <link name="left_arm_front_link"/>
        <link name="left_arm_back_link"/>
        <link name="right_arm_front_link"/>
        <link name="right_arm_back_link"/>
        <link name="left_finger_link"/>
        <link name="right_finger_link"/>
        <link name="gripper_ee_link"/>
    </group>

    <group_start_state name="start_gripper" group="endeffector">
        <joint name="driver_arm_joint" value="0"/>
    </group_start_state>
    <!-- Home state for manipulator group -->
    <group_state name="home" group="manipulator">
        <joint name="elbow_joint" value="0" />
        <joint name="shoulder_lift_joint" value="0" />
        <joint name="shoulder_pan_joint" value="0" />
        <joint name="wrist_1_joint" value="0" />
        <joint name="wrist_2_joint" value="0" />
        <joint name="wrist_3_joint" value="0" />
    </group_state>

    <!-- Up state for manipulator group -->
    <group_state name="up" group="manipulator">
        <joint name="elbow_joint" value="0" />
        <joint name="shoulder_lift_joint" value="-1.5707" />
        <joint name="shoulder_pan_joint" value="0" />
        <joint name="wrist_1_joint" value="-1.5707" />
        <joint name="wrist_2_joint" value="0" />
        <joint name="wrist_3_joint" value="0" />
    </group_state>
    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
    <end_effector name="moveit_ee" parent_link="ee_link" group="endeffector" parent_group="manipulator"/>
    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)
    <virtual_joint name="fixed_base" type="fixed" parent_frame="world" child_link="base_link" />
-->
    <!--DISABLE COLLISIONS: Within the robot-->
    <disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent" />
    <disable_collisions link1="ee_link" link2="wrist_1_link" reason="Never" />
    <disable_collisions link1="ee_link" link2="wrist_2_link" reason="Never" />
    <disable_collisions link1="ee_link" link2="wrist_3_link" reason="Adjacent" />
    <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent" />
    <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent" />
    <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent" />
    <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent" />
    <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never" />
    <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent" />
    <disable_collisions link1="gripper_ee_link" link2="shoulder_link" reason="Never"/>
    <disable_collisions link1="gripper_ee_link" link2="forearm_link" reason="Never"/>
    <disable_collisions link1="gripper_ee_link" link2="upper_arm_link" reason="Never"/>
    <disable_collisions link1="gripper_ee_link" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="gripper_ee_link" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="gripper_ee_link" link2="wrist_3_link" reason="Never"/>
    <!-- DISABLE COLLISIONS: Within the end-effector -->
    <disable_collisions link1="ee_link" link2="mount_link" reason="Adjacent"/>
    <disable_collisions link1 ="wrist_3_link" link2="mount_link" reason="Adjacent"/>
    <disable_collisions link1="mount_link" link2="plate_link" reason="Adjacent"/>
    <disable_collisions link1="plate_link" link2="driver_arm_link" reason="Adjacent"/>
    <disable_collisions link1="plate_link" link2="driven_arm_link" reason="Adjacent"/>
    <disable_collisions link1="plate_link" link2="left_arm_front_link" reason="Adjacent"/>
    <disable_collisions link1="plate_link" link2="left_arm_back_link" reason="Adjacent"/>
    <disable_collisions link1="plate_link" link2="right_arm_front_link" reason="Adjacent"/>
    <disable_collisions link1="plate_link" link2="right_arm_back_link" reason="Adjacent"/>
    <disable_collisions link1="driver_arm_link" link2="driven_arm_link" reason="Adjacent"/>
    <disable_collisions link1="left_arm_front_link" link2="left_finger_link" reason="Adjacent"/>
    <disable_collisions link1="left_arm_back_link" link2="left_finger_link" reason="Adjacent"/>
    <disable_collisions link1="right_arm_front_link" link2="right_finger_link" reason="Adjacent"/>
    <disable_collisions link1="right_arm_back_link" link2="right_finger_link" reason="Adjacent"/>
</robot>

Kinectv2 not working with real ur5 but does in simulation

Hi everyone,

I am using the UR5 with a own designed gripper as end-effector and KinectV2 to 'map' the environment so that the UR5 will avoid collisions with a human it might be working with. I have run some tests:

  • Simulated UR5 and simulated Kinect in gazebo. I can see the 'octomap' getting added to the planning scene in moveit.rviz and the UR5 avoids collisions with object placed in Gazebo.
  • Simulated UR5 in Gazebo and real Kinect, same as above.
  • Real UR5 and real Kinect. I cannot see the planning scene being added in moveit.rviz.

I get a warning from moveit that says:

[ WARN] [1516619887.119811534]: The complete state of the robot is not yet known.  Missing driver_arm_joint

Where driver_arm_joint is the joint that controls the gripper state.

My suggestion is that moveit cannot use the planning scene it is creating because the complete state of the robot is not yet known.

UPDATE: My gripper also becomes uncontrollable when launching the real UR5 and Kinectv2.

FINAL UPDATE: First I remapped the topic /joint_states (published by ur_modern_driver) to /joint_states_ur5. Then I wrote a node that subscribes to the /joint_states_ur5 topic and add a sensors_msgs/JointState message to the tuple of the gotten published by the ur_driver. Then I republish the message under the topic /joint_states. The python node is shown below.

import math
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Bool


class ur5_republisher():
    def __init__(self):
        rospy.init_node('Cmd_Scaler')
        self.pub = rospy.Publisher('/joint_states', JointState, queue_size=10)
        rospy.Subscriber("/joint_states_ur5", JointState, self.joint_state_callback)

        self.name = 'driver_arm_joint'
        self.position = 0
        self.velocity = 0
        self.effort = 0

    def appendT(self,t,x):
    l = list(t)
    l.append(x)
    return tuple(l)

    def joint_state_callback(self, data):
        data.name.append(self.name)
    data.position = self.appendT(data.position, self.position)
    data.velocity = self.appendT(data.velocity, self.position)
    data.effort = self.appendT(data.effort, self.position)
       self.pub.publish(data)

if __name__ == '__main__':
    ur5_r = ur5_republisher()
    while not rospy.is_shutdown():
    # self.cycle()
    rospy.spin()

My question is: Is there a way to initialize the endeffector group such that you define a start state? Currently this is the content of the srdf:

<?xml version="1.0" ?>
<!--This does not replace URDF, and is not an extension of URDF.
    This is a format for representing semantic information about the robot structure.
    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="ur5">
    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
    <group name="manipulator">
        <chain base_link="base_link" tip_link="ee_link" />
    </group>

    <group name="endeffector">
        <link name="mount_link"/>
        <link name="plate_link"/>
        <link name="driver_arm_link"/>
        <link name="driven_arm_link"/>
        <link name="left_arm_front_link"/>
        <link name="left_arm_back_link"/>
        <link name="right_arm_front_link"/>
        <link name="right_arm_back_link"/>
        <link name="left_finger_link"/>
        <link name="right_finger_link"/>
        <link name="gripper_ee_link"/>
    </group>

    <group_start_state name="start_gripper" group="endeffector">
        <joint name="driver_arm_joint" value="0"/>
    </group_start_state>
    <!-- Home state for manipulator group -->
    <group_state name="home" group="manipulator">
        <joint name="elbow_joint" value="0" />
        <joint name="shoulder_lift_joint" value="0" />
        <joint name="shoulder_pan_joint" value="0" />
        <joint name="wrist_1_joint" value="0" />
        <joint name="wrist_2_joint" value="0" />
        <joint name="wrist_3_joint" value="0" />
    </group_state>

    <!-- Up state for manipulator group -->
    <group_state name="up" group="manipulator">
        <joint name="elbow_joint" value="0" />
        <joint name="shoulder_lift_joint" value="-1.5707" />
        <joint name="shoulder_pan_joint" value="0" />
        <joint name="wrist_1_joint" value="-1.5707" />
        <joint name="wrist_2_joint" value="0" />
        <joint name="wrist_3_joint" value="0" />
    </group_state>
    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
    <end_effector name="moveit_ee" parent_link="ee_link" group="endeffector" parent_group="manipulator"/>
    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)
    <virtual_joint name="fixed_base" type="fixed" parent_frame="world" child_link="base_link" />
-->
    <!--DISABLE COLLISIONS: Within the robot-->
    <disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent" />
    <disable_collisions link1="ee_link" link2="wrist_1_link" reason="Never" />
    <disable_collisions link1="ee_link" link2="wrist_2_link" reason="Never" />
    <disable_collisions link1="ee_link" link2="wrist_3_link" reason="Adjacent" />
    <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent" />
    <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent" />
    <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent" />
    <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent" />
    <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never" />
    <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent" />
    <disable_collisions link1="gripper_ee_link" link2="shoulder_link" reason="Never"/>
    <disable_collisions link1="gripper_ee_link" link2="forearm_link" reason="Never"/>
    <disable_collisions link1="gripper_ee_link" link2="upper_arm_link" reason="Never"/>
    <disable_collisions link1="gripper_ee_link" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="gripper_ee_link" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="gripper_ee_link" link2="wrist_3_link" reason="Never"/>
    <!-- DISABLE COLLISIONS: Within the end-effector -->
    <disable_collisions link1="ee_link" link2="mount_link" reason="Adjacent"/>
    <disable_collisions link1 ="wrist_3_link" link2="mount_link" reason="Adjacent"/>
    <disable_collisions link1="mount_link" link2="plate_link" reason="Adjacent"/>
    <disable_collisions link1="plate_link" link2="driver_arm_link" reason="Adjacent"/>
    <disable_collisions link1="plate_link" link2="driven_arm_link" reason="Adjacent"/>
    <disable_collisions link1="plate_link" link2="left_arm_front_link" reason="Adjacent"/>
    <disable_collisions link1="plate_link" link2="left_arm_back_link" reason="Adjacent"/>
    <disable_collisions link1="plate_link" link2="right_arm_front_link" reason="Adjacent"/>
    <disable_collisions link1="plate_link" link2="right_arm_back_link" reason="Adjacent"/>
    <disable_collisions link1="driver_arm_link" link2="driven_arm_link" reason="Adjacent"/>
    <disable_collisions link1="left_arm_front_link" link2="left_finger_link" reason="Adjacent"/>
    <disable_collisions link1="left_arm_back_link" link2="left_finger_link" reason="Adjacent"/>
    <disable_collisions link1="right_arm_front_link" link2="right_finger_link" reason="Adjacent"/>
    <disable_collisions link1="right_arm_back_link" link2="right_finger_link" reason="Adjacent"/>
</robot>