# [SOLVED]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 ...
edit retag close merge delete

re: your edit: take a look at the source_list parameter of the wiki/joint_state_publisher.

( 2018-01-23 10:01:56 -0600 )edit

That way it's a bit more obvious where the question ends and the answer starts.

( 2018-01-23 10:02:45 -0600 )edit

@gvdhoorn Thank you for reminding me of the source_list param. I gave an answer to my own question like you asked. Altough I can't accept my own answer as correct because I don't have enough 'karma'. Would be nice if you could do that, if you agree.

( 2018-01-29 05:04:31 -0600 )edit

Sort by » oldest newest most voted

I was able to find the problem and solve it: I found out that the error I was having came from line 1188 from this header, http://docs.ros.org/jade/api/moveit_r...

Then I realized that the ur_modern_driver is keeping the /joint_states topic occupied so I remapped the /joint_states topic to /joint_states_ur5 by adding this line to the spawning of the ur_driver node(ur_modern_driver/launch/ur_common.launch):

<node name="ur_driver" pkg="ur_modern_driver" type="ur_driver" output="screen">
....
<remap from="joint_states" to="joint_states_ur5"/> <!-- THIS -->
....
</node>


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(FIRST NODE).

NOTE: For the updated python node (that includes setting the self.position based on a boolean rosparam look for the SECOND NODE).

FIRST NODE:

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.velocity)
data.effort = self.appendT(data.effort, self.effort)
self.pub.publish(data)

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


SECOND NODE:

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 = self.checkGripperState()
self.velocity = 0
self.effort = 0
def appendT(self,t,x):
l = list(t)
l.append(x)
return tuple(l)
def checkGripperState(self):
try:
gripper_state = rospy.get_param('/bool_gripperOpen_param')
if(gripper_state):
gripper_position = 0.25
return gripper_position
if not(gripper_state):
gripper_position = 0.0
return gripper_position
except KeyError:
gripper_state = 0.0
print("rosparam, bool_gripperOpen_param not set")
return gripper_state
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.velocity)
data.effort = self.appendT(data.effort, self.effort)
self.pub.publish(data)

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

more

Can you explain why you did not have to remap the joint_states topic when working with a simulated UR5 and gripper? Also, what do you mean by " keeping the /joint_states topic occupied?" I ask because it seems like the mobile base on my UR10 application is not getting to publish its joint states.

( 2018-03-08 14:19:35 -0600 )edit

@raequin When one is using a simulated robot. Using the URDF (where the gripper is implemented) all the states are part of the joint_states topic. However, when working with the real UR5 the ur_driver.cpp doesn't include the implemented gripper into the joint_states topic the ur_driver publishes

( 2018-03-11 11:11:22 -0600 )edit

@raequin I would suggest you run the 'rostopic echo joint_states' command . From there you would be able to see if your mobile base is added to the tuple on the 'joint_states' topic.

( 2018-03-11 11:13:28 -0600 )edit

Hi. Is there an error in the following?

    data.position = self.appendT(data.position, self.position)
data.velocity = self.appendT(data.velocity, self.position)
data.effort = self.appendT(data.effort, self.position)


It seems like it should be

    data.position = self.appendT(data.position, self.position)
data.velocity = self.appendT(data.velocity, self.velocity)
data.effort = self.appendT(data.effort, self.effort)


edit: My intention was for this to just be a comment on the accepted answer, not a separate answer. Oops.

more

@raequin Yes! You are completely right, thanks for pointing it out. I will edit the post.

( 2018-03-08 14:02:52 -0600 )edit