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()
class TestOrOctomap: """ """ def __init__(self): self.pub_marker = velmautils.MarkerPublisher() def spin(self): simulation = True rospack = rospkg.RosPack() env_file=rospack.get_path('velma_scripts') + '/data/key/vis_test.env.xml' xacro_uri=rospack.get_path('velma_description') + '/robots/velma.urdf.xacro' srdf_path=rospack.get_path('velma_description') + '/robots/' rrt = rrt_star_connect_planner.PlannerRRT(3, env_file, xacro_uri, srdf_path) print "creating interface for Velma..." # create the interface for Velma robot self.velma = Velma() print "done." rospy.sleep(0.5) self.velma.updateTransformations() if simulation: hv = [3.2, 3.2, 3.2, 3.2] ht = [3000, 3000, 3000, 3000] self.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) self.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) rospy.sleep(1.0) # # Initialise Openrave # openrave = openraveinstance.OpenraveInstance() openrave.startOpenraveURDF(env_file=env_file) openrave.readRobot(xacro_uri=xacro_uri, srdf_path=srdf_path) rrt.waitForInit() openrave.setCamera(PyKDL.Vector(2.0, 0.0, 2.0), PyKDL.Vector(0.60, 0.0, 1.10)) openrave.updateRobotConfigurationRos(self.velma.js_pos) # TEST: key hole goal if False: task = KeyRotTaskRRT(openrave) task.SampleGoal(None, None) exit(0) # TEST: head IK if False: v_rot = 0.800 v_lean = 0.375 v_head = 0.392 h_cam = 0.0 v_cam = 0.225 head_kin = headkinematics.HeadKinematics(v_rot, v_lean, v_head, h_cam, v_cam) openrave.addSphere("target_sphere", 0.05) while not rospy.is_shutdown(): head_kin.UpdateTorsoPose(openrave.robot_rave.GetJoint("torso_0_joint").GetValue(0), openrave.robot_rave.GetJoint("torso_1_joint").GetValue(0)) target_pos = PyKDL.Vector(1, random.uniform(-1.5, 1.5), random.uniform(0,2)) head_kin.UpdateTargetPosition(target_pos.x(), target_pos.y(), target_pos.z()) openrave.updatePose("target_sphere", PyKDL.Frame(target_pos)) head_kin.TransformTargetToHeadFrame() joint_pan, joint_tilt = head_kin.CalculateHeadPose() if joint_pan == None: continue openrave.robot_rave.SetDOFValues([joint_pan, joint_tilt], [openrave.robot_rave.GetJoint("head_pan_joint").GetDOFIndex(), openrave.robot_rave.GetJoint("head_tilt_joint").GetDOFIndex()]) raw_input("Press ENTER to continue...") exit(0) raw_input("Press ENTER to continue...") # T_B_E_list = [PyKDL.Frame(PyKDL.Rotation.RotY(90.0/180.0*math.pi), PyKDL.Vector(0.6, 0, 1.6))] # path, dof_names = rrt.RRTstar(openrave.robot_rave.GetDOFValues(), tasks.GraspTaskRRT, ("right", T_B_E_list), 30.0) path, dof_names = rrt.RRTstar(openrave.robot_rave.GetDOFValues(), tasks.KeyRotTaskRRT, ("right",), 30.0) # path, dof_names = rrt.RRTstar(openrave.robot_rave.GetDOFValues(), tasks.LooAtTaskRRT, ("right",), 60.0) # path, dof_names = rrt.RRTstar(openrave.robot_rave.GetDOFValues(), tasks.MoveArmsCloseTaskRRT, ("right",), 30.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 exit") == 'e': break openrave.showTrajectory(dof_names, 10.0, traj) rrt.cleanup() # raw_input("Press ENTER to exit...") exit(0) rospy.sleep(1) openrave.runOctomap() sphere = RaveCreateKinBody(openrave.env,'') sphere.SetName("sphere") sphere.InitFromSpheres(numpy.array([[0,0,0,0.05]]),True) openrave.env.Add(sphere,True) # test the collision checker for octomap if True: raw_input("Press ENTER to continue...") ob = openrave.env.GetKinBody("_OCTOMAP_MAP_") cc = openrave.env.GetCollisionChecker() m_id = 0 for x in np.linspace(0,1.5,30): for y in np.linspace(-1,1,40): for z in np.linspace(1,2,20): # print x,y,z tr = conv.KDLToOpenrave(PyKDL.Frame(PyKDL.Vector(x,y,z))) sphere.SetTransform(tr) openrave.env.UpdatePublishedBodies() report = CollisionReport() ret = cc.CheckCollision(sphere, report) # ret = openrave.env.CheckCollision(ob, report) # print ret if ret: m_id = self.pub_marker.publishSinglePointMarker(PyKDL.Vector(x,y,z), m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(0.1, 0.1, 0.1), T=None) continue if report.plink1 == None: print None else: print report.plink1.GetParent().GetName(), report.plink2.GetName() # print " ", report.vLinkColliding for link1, link2 in report.vLinkColliding: print " ", link1.GetParent().GetName(), link2.GetName() # print report.plink1.GetParent().GetName(), report.plink2.GetParent().GetName() exit(0) self.pub_head_look_at = rospy.Publisher("/head_lookat_pose", geometry_msgs.msg.Pose) raw_input("Press ENTER to look around...") # self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(0.2,-0.5,1)))) # raw_input("Press ENTER to exit...") # exit(0) raw_input("Press ENTER to look around...") self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,0,2)))) raw_input("Press ENTER to look around...") self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(0.2,1,2)))) raw_input("Press ENTER to look around...") self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(0.2,1,1.2)))) raw_input("Press ENTER to look around...") self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,0,1.2)))) raw_input("Press ENTER to look around...") self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(0.2,-1,1.2)))) raw_input("Press ENTER to look around...") self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(0.2,-1,2)))) raw_input("Press ENTER to look around...") self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,0,2)))) raw_input(".") exit(0)