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
