approach pose from z axis, relative to pose orientation
Using ROS1 Kinetic, programming in Python
I have a goal_pose my 6DOF robot arm can go to this pose. this pose is in the same frame as my robot (/base_link). I want to approach my goal_pose, but in the direction of the goal_pose, not the direction of my frame.
Image to illistrate the situation: https://i.imgur.com/9fWzW8Y.png
blue dot: desired pose
yellow dot: the pose reached when I do: goal_pose.z = goal_pose.z + 0.1 (this happends because it's doing the z-offset relative to the base_link)
I think I need to do a transform:
approach_pose_goal = pose_goal
tf_buffer = tf2_ros.Buffer(rospy.Duration(2.0))
tf2_ros.TransformListener(tf_buffer)
translation = (pose_goal.position.x, pose_goal.position.y, pose_goal.position.z)
rotation = (pose_goal.orientation.x, pose_goal.orientation.y, pose_goal.orientation.z,
approach_pose_goal.orientation.w)
br = tf.TransformBroadcaster()
br.sendTransform(translation, rotation, rospy.Time.now(), 'object', 'base_link')
try:
transform = tf_buffer.lookup_transform('base_link','object',
rospy.Time(0), rospy.Duration(1))
except:
rospy.logerr('Unable to find the transformation from source_frame to target_frame')
initial_pose = PoseStamped()
initial_pose.pose.position.z = 0.1
translated_pose = tf2_geometry_msgs.do_transform_pose(initial_pose, transform)
The problem here is that I need 2 frames, but I only have one frame (base_link). I tried to add my frame using this tutorial. But without succes.
I don't know if this is because my lack of understanding of tf. or that I'm approaching this wrong. So I hope someone could give me some insight.
I found these relevant topics, but in most cases the goal_pose has it's own frame, so a transform can be done:
construct a vector from quaternion
make end effector look at object
I hope my question is clear and I can get some guidance in the right direction.