ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org
Ask Your Question
0

approach pose from z axis, relative to pose orientation

asked 2021-05-06 04:31:32 -0600

washichi gravatar image

updated 2021-05-17 06:47:53 -0600

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

get vector of axis from pose

get desired coordinates

I hope my question is clear and I can get some guidance in the right direction.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-06-07 05:19:11 -0600

washichi gravatar image

I solved it by publishing my goal_pose as a tf_broadcaster:

br = tf2_ros.TransformBroadcaster()

t.header.stamp = rospy.Time.now()
t.header.frame_id = "world"
t.child_frame_id = "object"
t.transform.translation.x = pose_goal.position.x
t.transform.translation.y = pose_goal.position.y
t.transform.rotation.z = pose_goal.position.z
t.transform.rotation.x = pose_goal.orientation.x
t.transform.rotation.y = pose_goal.orientation.y
t.transform.rotation.z = pose_goal.orientation.z
t.transform.rotation.w = pose_goal.orientation.w

After my goal_pose is broadcasted I could lookup and do the transform:

   #lookup transform
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)

try:
    transform = tfBuffer.lookup_transform('world', 'object', rospy.Time(), rospy.Duration(1.0))
#except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
except Exception as e:
    print("transform failed")
    print(e)

initial_pose = PoseStamped()
initial_pose.pose.position.z = 0.1
translated_pose = tf2_geometry_msgs.do_transform_pose(initial_pose, transform)

The result is my translated_pose.

It turned out pretty easy with a tf_broadcaster, it's not very intiutive to me that you can't (or at least I didn't figure out how) transform without doing a broadcast.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-05-06 04:31:32 -0600

Seen: 207 times

Last updated: Jun 07 '21