OpenRAVE Documentation

tutorial_grasptransform Module

This tutorial shows how to find the transform that moves the hand to the target.

../../_images/tutorial_grasptransform.jpg

Running the Example:

openrave.py --example tutorial_grasptransform

Description

Initialize the environment

# set up planning environment
env = Environment()
env.SetViewer('qtcoin')

Load the robot and object

robot = env.ReadRobotXMLFile('robots/pr2-beta-static.zae')
env.Add(robot)
target = env.ReadKinBodyXMLFile('data/mug2.kinbody.xml')
env.Add(target)

Move the object

# init target pose
O_T_Target = array([[1,0,0,1],
                    [0,1,0,1],
                    [0,0,1,1],
                    [0,0,0,1]])
target.SetTransform(O_T_Target)

Open arms, raise torso, open gripper

# init robot pose
v = robot.GetActiveDOFValues()
v[35] = 3.14/2 # l shoulder pan
v[56] = -3.14/2 # r shoulder pan
v[14] = .31 # torso
v[47] = .54 # l gripper
robot.SetActiveDOFValues(v)

Doing transform

Given:

O_T_R = self.robot.GetTransform() # robot transform R in global frame O 
O_T_G = self.robot.GetActiveManipulator().GetTransform() # grasping frame G in global frame O

Want:

O_T_R_goal # final robot transform R_goal in global frame O

Solution:

G_T_O = linalg.inv(O_T_G) # global frame O in grasping frame G
G_T_R = dot(G_T_O, O_T_R) # robot frame R in grasping frame G
O_T_G_goal = self.robot.GetTransform() # final grasping frame G_goal in global frame O 
O_T_R_goal = dot(O_T_G_goal,G_T_R) # final robot transform R_goal in global frame O
../../_images/tutorial_grasptransform_O_T_R.png

robot frame R in global frame O

../../_images/tutorial_grasptransform_O_T_Target.png

target frame Target in global frame O

../../_images/tutorial_grasptransform_O_T_G.png

grasping frame G in global frame O

../../_images/tutorial_grasptransform_final.png

Robot goal configuration

Questions/Feedback

Having problems with OpenRAVE?