def main(env,options):
"Main example code."
robot = env.ReadRobotXMLFile(options.robot)
env.Add(robot)
if options.manipname is not None:
robot.SetActiveManipulator(options.manipname)
else:
robot.SetActiveManipulator('leftarm')
target = env.ReadKinBodyXMLFile(options.target)
env.Add(target)
# initialize target pose, for visualization and collision checking purpose only
O_T_Target = mat([[1,0,0,1],[0,1,0,0],[0,0,1,.9],[0,0,0,1]])
target.SetTransform(array(O_T_Target))
# set up goal grasp transform
# goal grasp transform specified in global frame, this equals manip.GetTransform() in the goal state
O_T_grasp = array([[ -9.88017917e-01, -1.54339954e-01 , 0.00000000e+00 , 1.06494129e+00],
[ 1.54339954e-01, -9.88017917e-01 , 0.00000000e+00 , 5.51449812e-05],
[ 0.00000000e+00 , 0.00000000e+00 , 1.00000000e+00 , 9.55221763e-01],
[ 0.00000000e+00 , 0.00000000e+00, 0.00000000e+00 , 1.00000000e+00]])
gripper_angle = .1
# use inversereachability dabase to find the possible robot base poses for the grasp
gr = InverseReachabilityDemo(robot)
gr.showPossibleBasePoses(O_T_grasp,gripper_angle,10)