Range of acceptable positions relative to a link
Hi
I have a Gazebo simulation of a robot arm with a parallel gripper (PhantomX Pincher Arm). Because grasping objects in Gazebo is sadly not really realistic or easy, I want to check if the gripper is in the correct position, so that it would pick up the object if it would close the fingers.
I can retrieve in my (Python) program the position & orientation of the object I want to pick up (pickMeUp) through the /gazebo/link_state
topic. This topic also gives me one of the two finger links of the gripper. I now would like to write a condition to check if the finger link is in a acceptable range relative to the link of the object to pick up (or vice versa of course). So something like this:
if(pose_gripper_link.x < pose_pickMeUp_link.x + distance_in_x-direction_of_pickMeUp and ... (other conditions)):
print("grasping would most likely be successful")
Here's a sketch of the situation: https://www.dropbox.com/s/ih20pt9h3z6...
How could I achieve this?
Note that I can only retrieve the Pose of pickMeUp via the /gazebo/link_state
topic, as this link doesn't appear in my /tf-tree. I can get the links of the gripper through tf though, and get the transformation between the /world coordinate system and the gripper-link coordinate system with the following code:
listener = tf.TransformListener()
try:
listener.waitForTransform("/gripper_finger_base_link", "/world", rospy.Time(), rospy.Duration(4.0))
(trans, rot) = listener.lookupTransform("/gripper_finger_base_link", "/world", rospy.Time())
except (tf.LookupException, tf.ConnectivityException) as e:
print(e)
But I don't know how I can apply this transformation & rotation to the Pose of pickMeUp, so that its Pose would be described in the /gripper_finger_base_link
's coordinate system. I'm also not sure if this is of any use for me, as the /gazebo/link_states
already gives me (one) gripper link relative to the /world
coordinate system. I could also construct a unit vector (described in the /world
coordinate system) in the direction of the x-axis of the pickMeUp link, in order to have something like the distance_in_x-direction_of_pickMeUp
form above. This shouldn't be a big issue, but I don't want to reinvent the wheel if this functionality is already in tf (or somewhere else).