def spin(self):
print "creating interface for Velma..."
# create the interface for Velma robot
velma = Velma()
rospy.sleep(0.5)
print "done."
hv = [1.2, 1.2, 1.2, 1.2]
ht = [3000, 3000, 3000, 3000]
velma.moveHandLeft([15.0/180.0*math.pi, 15.0/180.0*math.pi, 15.0/180.0*math.pi, 0], hv, ht, 5000, True)
print "wainting for moveHandLeft complete..."
result = velma.waitForHandLeft()
print "result:", result.error_code
velma.moveHandRight([15.0/180.0*math.pi, 15.0/180.0*math.pi, 15.0/180.0*math.pi, 0], hv, ht, 5000, True)
print "wainting for moveHandRight complete..."
result = velma.waitForHandRight()
print "result:", result.error_code
velma.moveHandLeft([70.0/180.0*math.pi, 70.0/180.0*math.pi, 70.0/180.0*math.pi, 0], hv, ht, 5000, True)
print "wainting for moveHandLeft complete..."
result = velma.waitForHandLeft()
print "result:", result.error_code
velma.switchToCart()
velma.updateTransformations()
T_B_Wr1 = velma.T_B_Wr
T_B_Wr2 = PyKDL.Frame(PyKDL.Vector(0.2,0,0)) * T_B_Wr1
velma.moveWristRight(T_B_Wr2, 3.0, Wrench(Vector3(40,40,40), Vector3(14,14,14)), start_time=0.1)
print "wainting for moveWristRight complete..."
result = velma.waitForWristRight()
print "result:", result.error_code
velma.moveWristRight(T_B_Wr1, 3.0, Wrench(Vector3(40,40,40), Vector3(14,14,14)), start_time=0.1)
print "wainting for moveWristRight complete..."
result = velma.waitForWristRight()
print "result:", result.error_code
velma.updateTransformations()
T_B_Wl1 = velma.T_B_Wl
T_B_Wl2 = PyKDL.Frame(PyKDL.Vector(0,0,0.2)) * T_B_Wl1
velma.moveWristLeft(T_B_Wl2, 3.0, Wrench(Vector3(40,40,40), Vector3(14,14,14)), start_time=0.1)
print "wainting for moveWristLeft complete..."
result = velma.waitForWristLeft()
print "result:", result.error_code
velma.moveWristLeft(T_B_Wl1, 3.0, Wrench(Vector3(40,40,40), Vector3(14,14,14)), start_time=0.1)
print "wainting for moveWristLeft complete..."
result = velma.waitForWristLeft()
print "result:", result.error_code
raw_input("Press ENTER to exit...")
def spin(self):
sys.setrecursionlimit(200)
# generate the set of grasps for a jar
grasps_T_J_E = []
for angle_jar_axis in np.arange(0.0, math.pi*2.0, 10.0/180.0*math.pi):
for translation_jar_axis in np.linspace(-0.03, 0.03, 7):
T_J_E = PyKDL.Frame(PyKDL.Rotation.RotZ(90.0/180.0*math.pi)) * PyKDL.Frame(PyKDL.Rotation.RotX(angle_jar_axis)) * PyKDL.Frame(PyKDL.Vector(translation_jar_axis, 0, -0.17))
grasps_T_J_E.append( (T_J_E, T_J_E.Inverse()) )
T_J_E = PyKDL.Frame(PyKDL.Rotation.RotZ(-90.0/180.0*math.pi)) * PyKDL.Frame(PyKDL.Rotation.RotX(angle_jar_axis)) * PyKDL.Frame(PyKDL.Vector(translation_jar_axis, 0, -0.17))
grasps_T_J_E.append( (T_J_E, T_J_E.Inverse()) )
# TEST: OpenJarTaskRRT
if False:
task = tasks.OpenJarTaskRRT(None, ("right", grasps_T_J_E, ))
m_id = 0
for coord in task.valid:
xi,yi,zi = coord
x = plutl.x_set[xi]
y = plutl.y_set[yi]
z = plutl.z_set[zi]
rot_set = task.valid[coord]
size = float(len(rot_set))/40.0
m_id = self.pub_marker.publishSinglePointMarker(PyKDL.Vector(x,y,z), m_id, r=1.0, g=1*size, b=1*size, a=1, namespace='default', frame_id="torso_link2", m_type=Marker.SPHERE, scale=Vector3(0.05*size, 0.05*size, 0.05*size))
rospy.sleep(0.01)
exit(0)
rospack = rospkg.RosPack()
env_file=rospack.get_path('velma_scripts') + '/data/common/velma_room.env.xml'
# env_file=rospack.get_path('velma_scripts') + '/data/common/velma_room_obstacles.env.xml'
srdf_path=rospack.get_path('velma_description') + '/robots/'
obj_filenames = [
rospack.get_path('velma_scripts') + '/data/jar/jar.kinbody.xml'
]
rrt = rrt_star_connect_planner.PlannerRRT(3, env_file, obj_filenames, srdf_path)
rospy.wait_for_service('/change_state')
changeState = rospy.ServiceProxy('/change_state', state_server_msgs.srv.ChangeState)
self.listener = tf.TransformListener()
print "creating interface for Velma..."
# create the interface for Velma robot
velma = Velma()
self.pub_head_look_at = rospy.Publisher("/head_lookat_pose", geometry_msgs.msg.Pose)
print "done."
rospy.sleep(0.5)
velma.updateTransformations()
# switch to cartesian impedance mode...
# velma.switchToCart()
# exit(0)
velma.switchToJoint()
# TEST: moving in joint impedance mode
if False:
raw_input("Press ENTER to move the robot...")
velma.switchToJoint()
joint_names = [ "right_arm_4_joint" ]
q_dest = [ velma.js_pos["right_arm_4_joint"] - 10.0/180.0*math.pi ]
velma.moveJoint(q_dest, joint_names, 10.0, start_time=0.5)
traj = [[q_dest], None, None, [10.0]]
velma.moveJointTraj(traj, joint_names, start_time=0.5)
result = velma.waitForJoint()
print "moveJointTraj result", result.error_code
exit(0)
hv = [1.2, 1.2, 1.2, 1.2]
ht = [3000, 3000, 3000, 3000]
target_gripper = "right"
if target_gripper == "left":
velma.moveHandLeft([40.0/180.0*math.pi, 40.0/180.0*math.pi, 40.0/180.0*math.pi, 0], hv, ht, 5000, True)
velma.moveHandRight([120.0/180.0*math.pi, 120.0/180.0*math.pi, 120.0/180.0*math.pi, 0], hv, ht, 5000, True)
else:
velma.moveHandLeft([120.0/180.0*math.pi, 120.0/180.0*math.pi, 120.0/180.0*math.pi, 0], hv, ht, 5000, True)
velma.moveHandRight([40.0/180.0*math.pi, 40.0/180.0*math.pi, 40.0/180.0*math.pi, 0], hv, ht, 5000, True)
result = velma.waitForHandLeft()
print "waitForHandLeft result", result.error_code
result = velma.waitForHandRight()
print "waitForHandRight result", result.error_code
#
# Initialise Openrave
#
openrave = openraveinstance.OpenraveInstance()
openrave.startOpenrave(collision='fcl')
openrave.loadEnv(env_file)
openrave.runOctomapClient(ros=True)
openrave.readRobot(srdf_path=srdf_path)
for filename in obj_filenames:
body = openrave.env.ReadKinBodyXMLFile(filename)
body.Enable(False)
body.SetVisible(False)
openrave.env.Add(body)
mo_state = objectstate.MovableObjectsState(openrave.env, obj_filenames)
openrave.setCamera(PyKDL.Vector(2.0, 0.0, 2.0), PyKDL.Vector(0.60, 0.0, 1.10))
openrave.updateRobotConfigurationRos(velma.js_pos)
while True:
if self.listener.canTransform('torso_base', 'jar', rospy.Time(0)):
pose = self.listener.lookupTransform('torso_base', 'jar', rospy.Time(0))
T_B_J = pm.fromTf(pose)
break
print "waiting for planner..."
rrt.waitForInit()
print "planner initialized"
T_B_E_list = []
for T_J_E, T_E_J in grasps_T_J_E:
T_B_Ed = T_B_J * T_J_E
T_B_E_list.append(T_B_Ed)
print "grasps for jar:", len(T_B_E_list)
if True:
# look around
self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,0,1.2))))
rospy.sleep(3)
# self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,-1,1.2))))
# rospy.sleep(2)
# self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,1,1.2))))
# rospy.sleep(2)
# self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,0,1.2))))
# rospy.sleep(2)
raw_input("Press ENTER to start planning...")
openrave.updateOctomap()
tree_serialized = openrave.or_octomap_client.SendCommand("GetOcTree")
for i in range(10):
time_tf = rospy.Time.now()-rospy.Duration(0.5)
mo_state.update(self.listener, time_tf)
mo_state.updateOpenrave(openrave.env)
rospy.sleep(0.1)
print "octomap updated"
print "Planning trajectory to grasp the jar..."
env_state = (openrave.robot_rave.GetDOFValues(), mo_state.obj_map, tree_serialized, [])
path, dof_names = rrt.RRTstar(env_state, tasks.GraspTaskRRT, (target_gripper, T_B_E_list), 240.0)
traj = []
for i in range(len(path)-1):
q1 = path[i]
q2 = path[i+1]
for f in np.linspace(0.0, 1.0, 40):
traj.append( q1 * (1.0 - f) + q2 * f )
while True:
if raw_input("Type e to execute") == 'e':
break
openrave.showTrajectory(dof_names, 10.0, traj)
traj = velma.prepareTrajectory(path, velma.getJointStatesByNames(dof_names), dof_names, speed_mult=1.0)
velma.switchToJoint()
velma.moveJointTraj(traj, dof_names, start_time=0.5)
result = velma.waitForJoint()
print "moveJointTraj result", result.error_code
if result.error_code != 0:
exit(0)
raw_input("Press ENTER to continue...")
openrave.updateRobotConfigurationRos(velma.js_pos)
print "before grasp"
report = CollisionReport()
if openrave.env.CheckCollision(openrave.robot_rave, report):
print "collision"
else:
print "no collision"
report = CollisionReport()
if openrave.robot_rave.CheckSelfCollision(report):
print "self-collision"
else:
print "no self-collision"
raw_input("Press ENTER to close the gripper...")
if target_gripper == "left":
velma.moveHandLeft([90.0/180.0*math.pi, 90.0/180.0*math.pi, 90.0/180.0*math.pi, 0], hv, ht, 5000, True)
result = velma.waitForHandLeft()
print "moveHandLeft result", result.error_code
else:
velma.moveHandRight([90.0/180.0*math.pi, 90.0/180.0*math.pi, 90.0/180.0*math.pi, 0], hv, ht, 5000, True)
result = velma.waitForHandRight()
print "waitForHandRight result", result.error_code
openrave.updateRobotConfigurationRos(velma.js_pos)
print "after grasp"
report = CollisionReport()
if openrave.env.CheckCollision(openrave.robot_rave, report):
print "collision"
else:
print "no collision"
report = CollisionReport()
if openrave.robot_rave.CheckSelfCollision(report):
print "self-collision"
else:
print "no self-collision"
openrave.robot_rave.Grab(openrave.env.GetKinBody("jar"), openrave.robot_rave.GetLink(target_gripper+"_HandPalmLink"))
changeState("grasp "+target_gripper+"_HandPalmLink "+ "jar")
print "after grab"
report = CollisionReport()
if openrave.env.CheckCollision(openrave.robot_rave, report):
print "collision"
else:
print "no collision"
report = CollisionReport()
if openrave.robot_rave.CheckSelfCollision(report):
print "self-collision"
else:
print "no self-collision"
print "Planning trajectory to pull the jar from the cabinet..."
for i in range(10):
time_tf = rospy.Time.now()-rospy.Duration(0.5)
mo_state.update(self.listener, time_tf)
mo_state.updateOpenrave(openrave.env)
rospy.sleep(0.1)
env_state = (openrave.robot_rave.GetDOFValues(), mo_state.obj_map, tree_serialized, [["jar", target_gripper+"_HandPalmLink"]])
# path, dof_names = rrt.RRTstar(env_state, tasks.GraspTaskRRT, (target_gripper, T_B_E_list), 60.0)
path, dof_names = rrt.RRTstar(env_state, tasks.MoveArmsCloseTaskRRT, (None,), 120.0)
# path.reverse()
traj = []
for i in range(len(path)-1):
q1 = path[i]
q2 = path[i+1]
for f in np.linspace(0.0, 1.0, 40):
traj.append( q1 * (1.0 - f) + q2 * f )
while True:
if raw_input("Type e to execute") == 'e':
break
openrave.showTrajectory(dof_names, 10.0, traj)
traj = velma.prepareTrajectory(path, velma.getJointStatesByNames(dof_names), dof_names, speed_mult=1.0)
result = velma.moveJointTraj(traj, dof_names, start_time=0.5)
result = velma.waitForJoint()
print "moveJointTraj result", result.error_code
if result.error_code != 0:
exit(0)
raw_input("Press ENTER to continue...")
openrave.updateRobotConfigurationRos(velma.js_pos)
changeState("drop "+target_gripper+"_HandPalmLink")
raw_input("Press ENTER to exit...")
rrt.cleanup()
