def test_plan_to_pose(xyz, xyzw, leftright, robot, initial_state=build_robot_state(), planner_id='', target_link="%s_gripper_tool_frame"): manip = robot.GetManipulator(leftright + "arm") base_pose = initial_state.multi_dof_joint_state.poses[0] base_q = base_pose.orientation base_p = base_pose.position t = rave.matrixFromPose( [base_q.w, base_q.x, base_q.y, base_q.z, base_p.x, base_p.y, base_p.z]) robot.SetTransform(t) joint_solutions = ku.ik_for_link( rave.matrixFromPose(np.r_[xyzw[3], xyzw[:3], xyz]), manip, target_link % leftright[0], 1, True) if len(joint_solutions) == 0: print "pose is not reachable" return None m = build_joint_request(joint_solutions[0], leftright, robot, initial_state, planner_id=planner_id) response = None try: t1 = time.time() response = get_motion_plan(m).motion_plan_response t2 = time.time() except rospy.service.ServiceException: pass # assert isinstance(response, MotionPlanResponse) if args.pause_after_response: raw_input("press enter to continue") if response is not None: if response.error_code.val != 1: print "Planner returned error code", response.error_code.val traj = [ list(jtp.positions) for jtp in response.trajectory.joint_trajectory.points ] return dict(returned=True, safe=not check_traj(traj, manip, 100), traj=traj, planning_time=response.planning_time.to_sec(), error_code=response.error_code.val) else: return dict(returned=False)
def test_plan_to_pose(xyz, xyzw, leftright, robot, initial_state = build_robot_state(), planner_id='', target_link="%s_gripper_tool_frame"): manip = robot.GetManipulator(leftright + "arm") base_pose = initial_state.multi_dof_joint_state.poses[0] base_q = base_pose.orientation base_p = base_pose.position t = rave.matrixFromPose([base_q.w, base_q.x, base_q.y, base_q.z, base_p.x, base_p.y, base_p.z]) robot.SetTransform(t) joint_solutions = ku.ik_for_link(rave.matrixFromPose(np.r_[xyzw[3], xyzw[:3], xyz]), manip, target_link%leftright[0], 1, True) if len(joint_solutions) == 0: print "pose is not reachable" return None m = build_joint_request(joint_solutions[0], leftright, robot, initial_state, planner_id=planner_id) response = None try: t1 = time.time() response = get_motion_plan(m).motion_plan_response t2 = time.time() except rospy.service.ServiceException: pass # assert isinstance(response, MotionPlanResponse) if args.pause_after_response: raw_input("press enter to continue") if response is not None: if response.error_code.val != 1: print "Planner returned error code", response.error_code.val traj = [list(jtp.positions) for jtp in response.trajectory.joint_trajectory.points] return dict(returned = True, safe = not check_traj(traj, manip, 100), traj = traj, planning_time = response.planning_time.to_sec(), error_code = response.error_code.val) else: return dict(returned = False)
def callback(getreq): assert isinstance(getreq, ms.GetMotionPlanRequest) req = getreq.motion_plan_request getresp = ms.GetMotionPlanResponse() resp = getresp.motion_plan_response manip = GROUPMAP[req.group_name] update_rave_from_ros(robot, req.start_state.joint_state.position, req.start_state.joint_state.name) base_pose = req.start_state.multi_dof_joint_state.poses[0] base_q = base_pose.orientation base_p = base_pose.position t = rave.matrixFromPose([base_q.w, base_q.x, base_q.y, base_q.z, base_p.x, base_p.y, base_p.z]) robot.SetTransform(t) start_joints = robot.GetDOFValues(robot.GetManipulator(manip).GetArmIndices()) n_steps = 9 coll_coeff = 10 dist_pen = .04 inds = robot.GetManipulator(manip).GetArmJoints() alljoints = robot.GetJoints() names = arm_joint_names = [alljoints[i].GetName() for i in inds] for goal_cnt in req.goal_constraints: if len(goal_cnt.joint_constraints) > 0: assert len(goal_cnt.joint_constraints)==7 end_joints = np.zeros(7) for i in xrange(7): jc = goal_cnt.joint_constraints[i] dof_idx = arm_joint_names.index(jc.joint_name) end_joints[dof_idx] = jc.position waypoint_step = (n_steps - 1)// 2 joint_waypoints = [(np.asarray(start_joints) + np.asarray(end_joints))/2] if args.multi_init: leftright = req.group_name.split("_")[0] joint_waypoints.extend(get_postures(leftright)) success = False for waypoint in joint_waypoints: robot.SetDOFValues(start_joints, inds) d = { "basic_info" : { "n_steps" : n_steps, "manip" : manip, "start_fixed" : True }, "costs" : [ { "type" : "joint_vel", "params": {"coeffs" : [1]} }, { "type" : "continuous_collision", "params" : {"coeffs" : [coll_coeff],"dist_pen" : [dist_pen]} } ], "constraints" : [], "init_info" : { "type" : "given_traj" } } d["constraints"].append({ "type" : "joint", "name" : "joint", "params" : { "vals" : end_joints.tolist() } }) if args.multi_init: inittraj = np.empty((n_steps, 7)) inittraj[:waypoint_step+1] = mu.linspace2d(start_joints, waypoint, waypoint_step+1) inittraj[waypoint_step:] = mu.linspace2d(waypoint, end_joints, n_steps - waypoint_step) else: inittraj = mu.linspace2d(start_joints, end_joints, n_steps) print "this",inittraj print "other",mu.linspace2d(start_joints, end_joints, n_steps) d["init_info"]["data"] = [row.tolist() for row in inittraj] if len(goal_cnt.position_constraints) > 0: raise NotImplementedError s = json.dumps(d) t_start = time() prob = trajoptpy.ConstructProblem(s, env) result = trajoptpy.OptimizeProblem(prob) planning_duration_seconds = time() - t_start traj = result.GetTraj() if check_traj(traj, robot.GetManipulator(manip)): print "aw, there's a collision" else: print "no collisions" success = True break if not success: resp.error_code.val = FAILURE return resp resp.trajectory.joint_trajectory.joint_names = names for row in traj: jtp = tm.JointTrajectoryPoint() jtp.positions = row.tolist() jtp.time_from_start = rospy.Duration(0) resp.trajectory.joint_trajectory.points.append(jtp) mdjt = resp.trajectory.multi_dof_joint_trajectory mdjt.joint_names = ['world_joint'] mdjt.frame_ids = ['odom_combined'] mdjt.child_frame_ids = ['base_footprint'] mdjt.header = req.start_state.joint_state.header pose = gm.Pose() pose.orientation.w = 1 for _ in xrange(len(resp.trajectory.joint_trajectory.points)): jtp = mm.MultiDOFJointTrajectoryPoint() jtp.poses.append(pose) #for i in xrange(len(mdjs.joint_names)): #if mdjs.joint_names[i] == "world_joint": #world_joint_pose = mdjs.poses[i] #world_joint_frame_id = mdjs.frame_ids[i] #world_joint_child_frame_id = mdjs.child_frame_ids[i] #print "world_joint_frame_id", world_joint_frame_id #print "world_joint_child_frame_id", world_joint_child_frame_id #mdjt = resp.trajectory.multi_dof_joint_trajectory #mdjt.header.frame_id = req.start_state.joint_state.header.frame_id #mdjt.joint_names.append("world_joint") resp.error_code.val = SUCCESS resp.planning_time = rospy.Duration(planning_duration_seconds) resp.trajectory.joint_trajectory.header = req.start_state.joint_state.header resp.group_name = req.group_name resp.trajectory_start.joint_state = req.start_state.joint_state #resp.trajectory.multi_dof_joint_trajectory #resp.trajectory.multi_dof_joint_trajectory.header = req.start_state.joint_state.header getresp.motion_plan_response = resp return getresp