def to_pick(robot, qgrasp):
# Usage: compute the trajectory of gripper to pick up the box from container
# Params:
# robot: Information of the robot body
# qgrasp: IK solution of the gripple to pick up the box
traj = planning.plan_to_joint_configuration(robot, qgrasp)
return (traj)
def to_put(robot, qgoal):
# Usage: compute the trajectory of gripper to put down the box at destination
# Params:
# robot: Information of the robot body
# qgoal: IK solution of the gripple to put down the box
traj = planning.plan_to_joint_configuration(robot, qgoal)
return (traj)
)
statsmodel.autogenerate()
statsmodel.setRobotWeights()
statsmodel.setRobotResolutions(xyzdelta=0.01)
# Find a valid IK solution for grasping the cube
cube = env.GetKinBody('cube01')
cube_centroid = cube.ComputeAABB().pos()
Tgrasp = tr.euler_matrix(0, np.pi, 0)
Tgrasp[:3, 3] = cube_centroid
qgrasp = kinematics.find_closest_iksolution(robot, Tgrasp, iktype)
axes = []
axes.append(orpy.misc.DrawAxes(env, Tgrasp, dist=0.05))
# Move to the grasping pose
traj = planning.plan_to_joint_configuration(robot, qgrasp)
ros_traj = criros.conversions.ros_trajectory_from_openrave(
robot.GetName(), traj)
trajectory_controller.set_trajectory(ros_traj)
trajectory_controller.start()
robot.GetController().SetPath(traj)
trajectory_controller.wait()
# Grasp the cube
gripper_controller.command(0.05)
taskmanip.CloseFingers()
gripper_controller.wait()
robot.WaitForController(0)
robot.Grab(cube)
gripper_controller.grab('{0}::link'.format(cube.GetName()))
# Find a valid IK solution for the retreat pose