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)
try:
response = get_motion_plan(m)
if args.pause_after_response:
raw_input("press enter to continue")
except rospy.ServiceException:
return dict(returned = False)
mpr = response.motion_plan_response
if mpr.error_code.val != 1:
print "Planner returned error code", mpr.error_code.val
return dict(returned = True, error_code = mpr.error_code.val, safe=False)
else:
traj = [list(jtp.positions) for jtp in mpr.trajectory.joint_trajectory.points]
return dict(returned = True, safe = traj_is_safe(traj, robot, 100), traj = traj, planning_time = mpr.planning_time.to_sec(), error_code = mpr.error_code.val)
def single_trial(inittraj, use_discrete_collision):
s = make_trajopt_request(n_steps, coll_coeff, dist_pen, end_joints, inittraj, use_discrete_collision)
prob = trajoptpy.ConstructProblem(s, robot.GetEnv())
result = trajoptpy.OptimizeProblem(prob)
traj = result.GetTraj()
prob.SetRobotActiveDOFs()
return traj, traj_is_safe(traj, robot) #and (use_discrete_collision or traj_no_self_collisions(traj, robot))
def trajOptGetMotionPlan(robot,goal_config,env):
robot.GetActiveDOFValues()
manipulator = robot.SetActiveManipulator('leftarm_torso')
robot.SetActiveDOFs(manipulator.GetArmIndices())
joint_target = goal_config.tolist()
request = {
"basic_info" : {
"n_steps" : 10,
"manip" : "leftarm_torso", # see below for valid values
"start_fixed" : True # i.e., DOF values at first timestep are fixed based on current robot state
},
"costs" : [
{
"type" : "joint_vel", # joint-space velocity cost
"params": {"coeffs" : [1]} # a list of length one is automatically expanded to a list of length n_dofs
# also valid: [1.9, 2, 3, 4, 5, 5, 4, 3, 2, 1]
},
{
"type" : "collision",
"params" : {
"coeffs" : [20], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
"dist_pen" : [0.025] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
},
}
],
"constraints" : [
{
"type" : "joint", # joint-space target
"params" : {"vals" : joint_target } # length of vals = # dofs of manip
}
],
"init_info" : {
"type" : "straight_line", # straight line in joint space.
"endpoint" : joint_target
}
}
s = json.dumps(request) # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(s, env) # create object that stores optimization problem
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob) # do optimization
t_elapsed = time.time() - t_start
print result
print "optimization took %.3f seconds"%t_elapsed
from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem
ObjToG = env.GetKinBody("ObjToG")
ObjToG.Enable(False)
try:
if not traj_is_safe(result.GetTraj(), robot): # Check that trajectory is colision free
ObjToG.Enable(True)
return None,t_elapsed,"Failed"
else:
ObjToG.Enable(True)
return result.GetTraj(),t_elapsed,"Success"
except:
print 'exception here'
import pdb; pdb.set_trace()
def single_trial(inittraj, use_discrete_collision):
s = make_trajopt_request(n_steps, coll_coeff, dist_pen, end_joints,
inittraj, use_discrete_collision)
robot.SetActiveDOFValues(inittraj[0, :])
prob = trajoptpy.ConstructProblem(s, robot.GetEnv())
result = trajoptpy.OptimizeProblem(prob)
traj = result.GetTraj()
prob.SetRobotActiveDOFs()
return traj, traj_is_safe(
traj, robot
) #and (use_discrete_collision or traj_no_self_collisions(traj, robot))
def check_result(result, robot):
print "checking trajectory for safety and constraint satisfaction..."
success = True
if not traj_is_safe(result.GetTraj(), robot):
success = False
print "trajectory has a collision!"
abstol = 1e-3
for (name, val) in result.GetConstraints():
if (val > abstol):
success = False
print "constraint %s wasn't satisfied (%.2e > %.2e)"%(name, val, abstol)
return success
def check_result(result, robot):
print "checking trajectory for safety and constraint satisfaction..."
success = True
if not traj_is_safe(result.GetTraj(), robot):
success = False
print "trajectory has a collision!"
abstol = 1e-3
for (name, val) in result.GetConstraints():
if (val > abstol):
success = False
print "constraint %s wasn't satisfied (%.2e > %.2e)"%(name, val, abstol)
return success
def verify(datum, waypoints):
env[0].Load(datum.env_path)
try:
return np.random.random() < 0.1
finally:
cleanup()
try:
robot = env[0].GetRobots()[0]
robot.SetDOFValues(datum.init)
traj = [datum.init] + list(waypoints) + [datum.goal]
return 1 if check_traj.traj_is_safe(traj, robot) else 0
except Exception as e:
print "in verify:" + str(e)
return 0
def plan(init_dof, waypoints, goal_dof, env):
robot = env[0].GetRobots()[0]
robot.SetDOFValues(init_dof)
#init_traj = np.concatenate((
# interp(init_dof, waypoint, 10), interp(waypoint, goal_dof, 10)))
init_traj = waypoints
#init_traj = interp(init_dof, goal_dof, 20)
#init_traj = init_traj.tolist()
request = {
"basic_info" : {
"n_steps" : len(init_traj),
"manip" : "active",
"start_fixed" : True
},
"costs" : [
{
"type" : "joint_vel",
"params" : { "coeffs" : [1] }
},
{
"type" : "collision",
"params" : {
"coeffs" : [20],
"dist_pen" : [0.025]
},
}
],
"constraints" : [
{
"type" : "joint",
"params" : { "vals" : goal_dof }
}
],
"init_info" : {
"type" : "given_traj",
"data" : init_traj
}
}
#s = json.dumps(request)
#prob = trajoptpy.ConstructProblem(s, env)
#result = trajoptpy.OptimizeProblem(prob)
#traj = result.GetTraj()
traj = init_traj
if check_traj.traj_is_safe(traj, robot):
return 1
else:
return 0
def main():
env = openravepy.Environment()
env.Load(osp.join(trajoptpy.data_dir, "kitchen.env.xml"))
env.Load("robots/pr2-beta-static.zae")
env.StopSimulation()
robot = env.GetRobots()[0]
# Set up robot in initial state
robot.SetDOFValues([ 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00,
0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00,
0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00,
6.648e-01, -3.526e-01, 1.676e+00, -1.924e+00, 2.921e+00,
-1.217e+00, 1.343e+00, 0.000e+00, -4.163e-16, 0.000e+00,
-1.665e-16, 0.000e+00, -6.365e-01, 9.806e-02, -1.226e+00,
-2.026e+00, -3.012e+00, -1.396e+00, -1.929e+00, 0.000e+00,
2.776e-17, 0.000e+00, -3.331e-16, 0.000e+00])
robot.SetTransform(np.array([[-1. , 0.005, 0. , 2.93 ],
[-0.005, -1. , 0. , 0.575],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ]]))
DOF = openravepy.DOFAffine
robot.SetActiveDOFs(np.r_[robot.GetJoint("torso_lift_joint").GetDOFIndex(), robot.GetManipulator("leftarm").GetArmIndices(), robot.GetManipulator("rightarm").GetArmIndices()],
DOF.X | DOF.Y | DOF.RotationAxis)
robot.SetActiveDOFValues([ 0. , 0.6648, -0.3526, 1.6763, -1.9242, 2.9209, -1.2166,
1.3425, -0.6365, 0.0981, -1.226 , -2.0264, -3.0125, -1.3958,
-1.9289, 2.9295, 0.5748, -3.137 ])
target_joints = [ 0. , 0.6808, -0.3535, 1.4343, -1.8516, 2.7542, -1.2005,
1.5994, -0.6929, -0.3338, -1.292 , -1.9048, -2.6915, -1.2908,
-1.7152, 1.3155, 0.6877, -0.0041]
# Planner assumes current state of robot in OpenRAVE is the initial state
request = make_fullbody_request(target_joints)
s = json.dumps(request) # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(s, env) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
traj = result.GetTraj()
success = traj_is_safe(traj, robot)
if success:
print "trajectory is safe! :)"
else:
print "trajectory contains a collision :("
if args.interactive: animate_traj(traj, robot)
assert success
def sample_traj(self, traj_obj):
spec = traj_obj.GetConfigurationSpecification()
traj = []
step = traj_obj.GetDuration() / self.params.n_steps
for i in range(self.params.n_steps):
data = traj_obj.Sample(i * step)
T = spec.ExtractTransform(None, data, self.robot)
pose = rave.poseFromMatrix(T) # wxyz, xyz
euler = transformations.euler_from_quaternion(np.r_[pose[1:4], pose[0]])
traj.append(np.r_[pose[4:7], euler[0:3]])
with self.robot:
if check_traj.traj_is_safe(traj, self.robot):
return traj, traj_obj.GetDuration()
return None, None
def trajOptGetMotionPlan(env, goal_config):
collisionChecker = RaveCreateCollisionChecker(env, "ode")
env.SetCollisionChecker(collisionChecker)
robot = env.GetRobots()[0]
ObjToG = env.GetKinBody("ObjToG")
ObjToG.Enable(True)
manipulator = robot.SetActiveManipulator("leftarm_torso")
robot.SetActiveDOFs(manipulator.GetArmIndices())
joint_target = goal_config.tolist()
request = {
"basic_info": {"n_steps": 10, "manip": "leftarm_torso", "start_fixed": True},
"costs": [
{"type": "joint_vel", "params": {"coeffs": [1]}},
{"type": "collision", "params": {"coeffs": [20], "dist_pen": [0.025]}},
],
"constraints": [{"type": "joint", "params": {"vals": joint_target}}],
"init_info": {"type": "straight_line", "endpoint": joint_target},
}
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, env)
t_start = time.time()
try:
result = trajoptpy.OptimizeProblem(prob) # do optimization
except:
return None, 0, "Failed"
t_elapsed = time.time() - t_start
# print "optimization took %.3f seconds"%t_elapsed
from trajoptpy.check_traj import traj_is_safe
with robot:
prob.SetRobotActiveDOFs()
ObjToG = env.GetKinBody("ObjToG")
ObjToG.Enable(False)
if not traj_is_safe(result.GetTraj(), robot):
ObjToG.Enable(True)
return None, t_elapsed, "Failed"
else:
ObjToG.Enable(True)
return result.GetTraj(), t_elapsed, "Success"
def check_traj_pose(env, result, target_left, target_right, threshold=0.01):
#check trajectory with Cartesian target
robot = env.GetRobots()[0]
traj = result.GetTraj()
#check for collision
if traj_is_safe(traj, robot) is not True:
print "There is a collision within the trajectory!"
return False
#check optimal solver status
if (result.GetStatus()[0] is not 0):
print "Optimization is not converged!"
return False
robot.SetActiveDOFValues(traj[-1])
T_left = robot.GetLink('l_gripper_tool_frame').GetTransform()
T_right = robot.GetLink('r_gripper_tool_frame').GetTransform()
pose_left = np.concatenate(
[T_left[0:3, 3].flatten(),
openravepy.quatFromRotationMatrix(T_left)])
pose_right = np.concatenate([
T_right[0:3, 3].flatten(),
openravepy.quatFromRotationMatrix(T_right)
])
#check target for joint constraints
if (np.linalg.norm(target_left - pose_left) > threshold):
print target_left, pose_left
print('Left pose is not reached!')
return False
if (np.linalg.norm(target_right - pose_right) > threshold):
print target_right, pose_right
print('Right pose is not reached!')
return False
#Otherwise, return True
return True
def plan_with_inittraj(self, start, goal, inittraj=None):
if self.verbose:
draw.draw_trajectory(self.env, inittraj, colors=np.array((0.5, 0.5, 0.5)))
if self.params.n_steps > 2:
with self.robot:
self.robot.SetActiveDOFValues(start)
request = self.make_fullbody_request(goal, inittraj, self.params.n_steps)
prob = trajoptpy.ConstructProblem(json.dumps(request), self.env)
def constraint(dofs):
valid = True
if self.params.dof == 6:
valid &= abs(dofs[3]) < 0.1
valid &= abs(dofs[4]) < 0.1
with self.robot:
self.robot.SetActiveDOFValues(dofs)
valid &= not self.env.CheckCollision(self.robot)
return 0 if valid else 1
for t in range(1, self.params.n_steps):
prob.AddConstraint(
constraint, [(t, j) for j in range(self.params.dof)], "EQ", "constraint%i" % t)
result = trajoptpy.OptimizeProblem(prob)
traj = result.GetTraj()
prob.SetRobotActiveDOFs()
if traj is not None:
if self.verbose:
draw.draw_trajectory(self.env, traj)
if check_traj.traj_is_safe(traj, self.robot):
cost = sum(cost[1] for cost in result.GetCosts())
return traj, cost
elif self.params.n_steps <= 2:
return [start, goal], utils.dist(start, goal)
return None, None
def check_traj(env, result, target, threshold=0.01):
#check trajectory with the target as joint configuration
robot = env.GetRobots()[0]
traj = result.GetTraj()
#check for collision
if traj_is_safe(traj, robot) is not True:
print "There is a collision within the trajectory!"
return False
#check optimal solver status
if (result.GetStatus()[0] is not 0):
print "Optimization is not converged!"
return False
#todo: put condition on either pose or joint constraints
#check target for joint constraints
if (np.linalg.norm(target - traj[-1]) > threshold):
print('Target joint is not reached!')
return False
#Otherwise, return True
return True
# run chomp
msg = ''
t_start = time()
status = PlannerStatus.FAIL_COLLISION
traj = []
if args.multi_init:
for i_init, inittraj in enumerate(init_trajs):
t = kwargs["starttraj"] = array_to_traj(robot, inittraj)
try:
rave_traj = m_chomp.runchomp(**kwargs)
saver.Restore() # set active dofs to original (including affine), needed for traj_to_array
traj = traj_to_array(robot, rave_traj)
if not traj_in_joint_limits(traj, robot):
status = PlannerStatus.FAIL_JOINT_VIOLATION
elif not traj_is_safe(traj, robot):
status = PlannerStatus.FAIL_COLLISION
else:
status = PlannerStatus.SUCCESS
msg = "planning successful after %s initialization"%(i_init+1)
break
except Exception, e:
msg = "CHOMP failed with exception: %s" % str(e)
if 'limit' in msg:
status = PlannerStatus.FAIL_JOINT_VIOLATION
else:
status = PlannerStatus.FAIL_OTHER
continue
else:
kwargs["adofgoal"] = target_dof_values
try:
def optimize1(self, n_steps, trajStartJoints, trajEndJoints):
msg = TrajoptCall()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '/0_link'
startJointPositions = []
endJointPositions = []
startPoses = []
endPoses = []
toolFrames = []
manips = []
approachDirs = []
for armName in self.armNames:
endJoints = trajEndJoints[armName]
if endJoints is None:
print trajEndJoints, self.trajRequest
endJoints = dict([(jointType,jointPos) for jointType, jointPos in endJoints.items() if jointType in self.rosJointTypes])
startJoints = trajStartJoints[armName]
startJoints = dict([(jointType,jointPos) for jointType, jointPos in startJoints.items() if jointType in self.rosJointTypes])
print 'start joints %s: %s' % (armName, [startJoints[k] for k in sorted(startJoints.keys())])
print 'end joints %s: %s' % (armName, [endJoints[k] for k in sorted(endJoints.keys())])
for raveJointType in self.manip[armName].GetArmIndices():
rosJointType = self.raveJointTypesToRos[armName][raveJointType]
endJointPositions.append(endJoints[rosJointType])
startJointPositions.append(startJoints[rosJointType])
startPoses.append(self.trajStartPose[armName])
endPoses.append(self.trajEndPose[armName])
toolFrames.append(self.toolFrame[armName])
manips.append(self.manip[armName])
approachDirs.append(self.approachDir[armName])
self.updateOpenraveJoints(armName, trajStartJoints[armName])
if armName == 'L':
msg.start_L = self.trajStartPose[armName]
msg.end_L = self.trajEndPose[armName]
else:
msg.start_R = self.trajStartPose[armName]
msg.end_R = self.trajEndPose[armName]
#request = jointRequest(n_steps, endJointPositions)
request = jointRequest(n_steps, endJointPositions, startPoses, endPoses, toolFrames, manips, approachDirs=approachDirs, approachDist=0.03)
#IPython.embed()
# convert dictionary into json-formatted string
s = json.dumps(request)
#print s
#print self.robot.GetActiveDOFValues()
#return
# create object that stores optimization problem
rospy.loginfo('Constructing Problem')
prob = trajoptpy.ConstructProblem(s, self.env)
# do optimization
rospy.loginfo('Optimizing Problem')
result = trajoptpy.OptimizeProblem(prob)
# check trajectory safety
from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs()
if not traj_is_safe(result.GetTraj(), self.robot):
rospy.loginfo('Trajopt trajectory is not safe. Trajopt failed!')
for armName in self.armNames:
self.poseTraj[armName] = None
self.deltaPoseTraj[armName] = None
else:
startIndex = 0
for armName in self.armNames:
endIndex = startIndex + len(self.manipJoints[armName])
graspKwargs = {}
if self.trajStartGrasp[armName] is not None:
graspKwargs['startGrasp'] = self.trajStartGrasp[armName]
if self.trajEndGrasp[armName] is not None:
graspKwargs['endGrasp'] = self.trajEndGrasp[armName]
armJointTrajArray = result.GetTraj()[:,startIndex:endIndex]
jointTrajDicts = self.jointTrajToDicts(armName, armJointTrajArray, **graspKwargs)
self.jointTraj[armName] = jointTrajDicts # for debugging
poseTraj = self.jointDictsToPoses(armName, jointTrajDicts)
#if self.addNoise:
# poseTraj = self.perturbTrajectory(armName, poseTraj)
correctedPoseTraj = copy.copy(poseTraj) #self.correctPoseTrajectory(armName, poseTraj) # apply calibrated error model to trajectory
self.poseTraj[armName] = copy.copy(correctedPoseTraj)
deltaPoseTraj = self.posesToDeltaPoses(correctedPoseTraj)
self.deltaPoseTraj[armName] = deltaPoseTraj
startIndex = endIndex
if armName == 'L':
msg.traj_L = [p.msg.Pose() for p in self.poseTraj[armName]]
else:
msg.traj_R = [p.msg.Pose() for p in self.poseTraj[armName]]
self.trajopt_pub.publish(msg)
marker = Marker()
marker.header.frame_id = '/0_link'
marker.header.stamp = rospy.Time.now()
marker.type = Marker.POINTS
marker.action = Marker.ADD
marker.ns = 'trajopt'
marker.id = 0
marker.scale.x = 0.002
marker.scale.y = 0.002
marker.color.a = 0.75
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.5
for arm in self.poseTraj:
for p in self.poseTraj[arm]:
marker.points.append(p.position.msg.Point())
#marker.lifetime = rospy.Duration(1.5)
self.trajopt_marker_pub.publish(marker)
print 'FINISHED OPTIMIZATION'
}],
"constraints": [{
"type": "pose",
"params": {
"xyz": xyz_target,
"wxyz": quat_target,
"link": "Palm",
"timestep": 9
}
}],
"init_info": {
"type": "straight_line",
"endpoint": init_joint_target.tolist()
}
}
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, env)
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob)
t_elapsed = time.time() - t_start
print "optimization took %.3f seconds" % t_elapsed
prob.SetRobotActiveDOFs()
assert traj_is_safe(result.GetTraj(), robot)
traj = result.GetTraj()
for t in traj:
robot.SetDOFValues(t, manip.GetArmIndices())
time.sleep(0.5)
def optimize_trajopt(self, joint_target):
def __construct_problem(request):
jd = json.dumps(request) # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(jd, self.env) # create object that stores optimization problem
return trajoptpy.OptimizeProblem(prob) # do optimization
starting_DOF = self.get_manip_DOF()
n_steps = self.__get_n_steps_from_dist(starting_DOF, joint_target)
request = {
"basic_info" : {
"n_steps" : n_steps,
"manip" : "arm", # see below for valid values
"start_fixed" : True # i.e., DOF values at first timestep are fixed based on current robot state
},
"costs" : [
{
"type" : "joint_vel", # joint-space velocity cost
"params": {"coeffs" : [10]} # list of length 1 is will be expanded to all DOF
},
{
# Self collision checker
"type" : "collision",
"params" : {
"coeffs" : [1000],
"dist_pen" : [0.03], # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
"continuous" : True
}
},
{
"type" : "collision",
"params" : {
"coeffs" : [1000], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
"dist_pen" : [0.03], # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
"continuous" : False
}
}
],
"constraints" : [
{
"type" : "joint", # joint-space target
"params" : {"vals" : joint_target } # length of vals = # dofs of manip
},
{
"type" : "cart_vel",
"name" : "cart_vel",
"params" : {
"max_displacement" : 0.05,
"first_step" : 0,
"last_step" : n_steps-1, #inclusive
"link" : "link6"
},
}
],
"init_info" : {
"type" : "straight_line", # straight line in joint space.
"endpoint" : joint_target
# "type" : "stationary",
}
}
result = __construct_problem(request)
try:
assert traj_is_safe(result.GetTraj(), self.robot)
except AssertionError:
print("Straight line planning failed. Reverting to stationary planning")
self.get_robot().SetActiveDOFValues(starting_DOF) # Reset arm positioning
request["init_info"] = {"type" : "stationary"}
result = __construct_problem(request)
rospy.loginfo("No of points -> {0}".format(n_steps))
return result.GetTraj()
def PlanToTSR(self, robot, tsrchains, is_interactive=False, **kw_args):
"""
Plan using the given list of TSR chains with TrajOpt.
@param robot the robot whose active manipulator will be used
@param tsrchains a list of TSRChain objects to respect during planning
@param is_interactive pause every iteration, until you press 'p'
or press escape to disable further plotting
@return traj
"""
import json
import time
import trajoptpy
manipulator = robot.GetActiveManipulator()
n_steps = 20
n_dofs = robot.GetActiveDOF()
# Create separate lists for the goal and trajectory-wide constraints.
goal_tsrs = [t for t in tsrchains if t.sample_goal]
traj_tsrs = [t for t in tsrchains if t.constrain]
# Find an initial collision-free IK solution by sampling goal TSRs.
from openravepy import (IkFilterOptions,
IkParameterization,
IkParameterizationType)
for tsr in self._TsrSampler(goal_tsrs):
ik_param = IkParameterization(
tsr.sample(), IkParameterizationType.Transform6D)
init_joint_config = manipulator.FindIKSolutions(
ik_param, IkFilterOptions.CheckEnvCollisions)
if init_joint_config:
break
if not init_joint_config:
raise PlanningError('No collision-free IK solutions.')
# Construct a planning request with these constraints.
request = {
"basic_info": {
"n_steps": n_steps,
"manip": "active",
"start_fixed": True
},
"costs": [
{
"type": "joint_vel",
"params": {"coeffs": [1]}
},
{
"type": "collision",
"params": {
"coeffs": [20],
"dist_pen": [0.025]
},
}
],
"constraints": [],
"init_info": {
"type": "straight_line",
"endpoint": init_joint_config.tolist()
}
}
# Set up environment.
env = robot.GetEnv()
trajoptpy.SetInteractive(is_interactive)
# Convert dictionary into json-formatted string and create object that
# stores optimization problem.
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, env)
for t in xrange(1, n_steps):
prob.AddConstraint(constraints._TsrCostFn(robot, traj_tsrs),
[(t, j) for j in xrange(n_dofs)],
"EQ", "up{:d}".format(t))
prob.AddConstraint(constraints._TsrCostFn(robot, goal_tsrs),
[(n_steps-1, j) for j in xrange(n_dofs)],
"EQ", "up{:d}".format(t))
# Perform trajectory optimization.
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob)
t_elapsed = time.time() - t_start
logger.info("Optimization took {:.3f} seconds".format(t_elapsed))
# Check for constraint violations.
if result.GetConstraints():
raise PlanningError("Trajectory did not satisfy constraints: {:s}"
.format(str(result.GetConstraints())))
# Check for the returned trajectory.
waypoints = result.GetTraj()
if waypoints is None:
raise PlanningError("Trajectory result was empty.")
# Set active DOFs to match active manipulator and plan.
p = openravepy.KinBody.SaveParameters
with robot.CreateRobotStateSaver(p.ActiveDOF):
# Set robot DOFs to DOFs in optimization problem
prob.SetRobotActiveDOFs()
# Check that trajectory is collision free
from trajoptpy.check_traj import traj_is_safe
if not traj_is_safe(waypoints, robot):
return PlanningError("Result was in collision.")
# Convert the waypoints to a trajectory.
return self._WaypointsToTraj(robot, waypoints)
def __assert_route_safe(self, prob, result): from trajoptpy.check_traj import traj_is_safe prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem assert traj_is_safe(result.GetTraj(), self.robot) # Check that trajectory is collision free
def _Plan(self,
robot,
request,
traj_constraints=(),
goal_constraints=(),
traj_costs=(),
goal_costs=(),
interactive=False,
constraint_threshold=1e-4,
**kwargs):
"""
Plan to a desired configuration with Trajopt.
This function invokes the Trajopt planner directly on the specified
JSON request. This can be used to implement custom path optimization
algorithms.
Constraints and costs are specified as dicts of:
```
{
'f': [float] -> [float],
'dfdx': [float] -> [float],
'type': ConstraintType or CostType
'dofs': [int]
}
```
The input to f(x) and dfdx(x) is a vector of active DOF values used in
the planning problem. The output is a vector of costs, where the
value *increases* as a constraint or a cost function is violated or
unsatisfied.
See ConstraintType and CostType for descriptions of the various
function specifications and their expected behavior.
The `dofs` parameter can be used to specify a subset of the robot's
DOF indices that should be used. A ValueError is thrown if these
indices are not entirely contained in the current active DOFs of the
robot.
@param robot: the robot whose active DOFs will be used
@param request: a JSON planning request for Trajopt
@param traj_constraints: list of dicts of constraints that should be
applied over the whole trajectory
@param goal_constraints: list of dicts of constraints that should be
applied only at the last waypoint
@param traj_costs: list of dicts of costs that should be applied over
the whole trajectory
@param goal_costs: list of dicts of costs that should be applied only
at the last waypoint
@param interactive: pause every iteration, until you press 'p' or press
escape to disable further plotting
@param constraint_threshold: acceptable per-constraint violation error
@returns traj: trajectory from current configuration to specified goal
"""
import json
import trajoptpy
# Set up environment.
env = robot.GetEnv()
trajoptpy.SetInteractive(interactive)
# Trajopt's UserData gets confused if the same environment
# is cloned into multiple times, so create a scope to later
# remove all TrajOpt UserData keys.
try:
# Convert dictionary into json-formatted string and create object
# that stores optimization problem.
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, env)
assert (request['basic_info']['manip'] == 'active')
assert (request['basic_info']['n_steps'] is not None)
n_steps = request['basic_info']['n_steps']
n_dofs = robot.GetActiveDOF()
i_dofs = robot.GetActiveDOFIndices()
# Add trajectory-wide costs and constraints to each timestep.
for t in xrange(1, n_steps):
for constraint in traj_constraints:
self._addFunction(prob, t, i_dofs, n_dofs, constraint)
for cost in traj_costs:
self._addFunction(prob, t, i_dofs, n_dofs, cost)
# Add goal costs and constraints.
for constraint in goal_constraints:
self._addFunction(prob, n_steps - 1, i_dofs, n_dofs,
constraint)
for cost in goal_costs:
self._addFunction(prob, n_steps - 1, i_dofs, n_dofs, cost)
# Perform trajectory optimization.
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob)
t_elapsed = time.time() - t_start
logger.debug("Optimization took {:.3f} seconds".format(t_elapsed))
# Check for constraint violations.
for name, error in result.GetConstraints():
if error > constraint_threshold:
raise PlanningError(
"Trajectory violates contraint '{:s}': {:f} > {:f}".
format(name, error, constraint_threshold))
# Check for the returned trajectory.
waypoints = result.GetTraj()
if waypoints is None:
raise PlanningError("Trajectory result was empty.")
# Set active DOFs to match active manipulator and plan.
p = openravepy.KinBody.SaveParameters
with robot.CreateRobotStateSaver(p.ActiveDOF):
# Set robot DOFs to DOFs in optimization problem
prob.SetRobotActiveDOFs()
# Check that trajectory is collision free
from trajoptpy.check_traj import traj_is_safe
if not traj_is_safe(waypoints, robot):
raise PlanningError("Result was in collision.")
# Convert the waypoints to a trajectory.
return self._WaypointsToTraj(robot, waypoints)
finally:
for body in env.GetBodies():
for key in TRAJOPT_USERDATA_KEYS:
body.RemoveUserData(key)
trajopt_env_userdata = env.GetKinBody('__trajopt_data__')
if trajopt_env_userdata is not None:
env.Remove(trajopt_env_userdata)
"params": {
"vals": joint_target
} # length of vals = # dofs of manip
}],
"init_info": {
"type": "straight_line", # straight line in joint space.
"endpoint": joint_target
}
}
s = json.dumps(request) # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(
s, env) # create object that stores optimization problem
print "--------------------------------------"
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob) # do optimization
t_elapsed = time.time() - t_start
print result
print "optimization took %.3f seconds" % t_elapsed
from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem
assert traj_is_safe(result.GetTraj(),
robot) # Check that trajectory is collision free
print result.GetTraj()
print np.shape(result.GetTraj())
s = raw_input("Hit Enter to request grasps ")
def __init__(self, interactive):
InitOpenRAVELogging()
env = openravepy.Environment()
env.StopSimulation()
print "Loading robot model"
env.Load("model/model.xml")
print "loaded robot model"
print "Loading environment"
env.Load("environment/env.xml")
print "Environment loaded"
trajoptpy.SetInteractive(True) # pause every iteration, until you press 'p'. Press escape to disable further plotting
robot = env.GetRobots()[0]
robot.SetActiveManipulator("arm")
print robot.GetManipulator("arm")
print robot.GetManipulator('arm').GetArmIndices()
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(robot,
iktype=IkParameterizationType.TranslationXY2D)
if not ikmodel.load():
ikmodel.autogenerate()
joint_start = [0.0, 0.0, 0.0]
robot.SetDOFValues(joint_start, robot.GetManipulator('arm').GetArmIndices())
target=ikmodel.manip.GetTransform()[0:2,3]
target[0] = -2.7
solutions = ikmodel.manip.FindIKSolutions(IkParameterization(target,IkParameterization.Type.TranslationXY2D), False)
if not len(solutions) == 0:
joint_target = [solutions[0][0], solutions[0][1], solutions[0][1]]
else:
print "No IK solutions found for cartesian coordinates " + str(target)
return
control_rate = 30.0
delta_t = 1.0 / control_rate
max_joint_velocity = 2.0
n_steps = 300
request = {
"basic_info" : {
"n_steps" : n_steps,
"manip" : "arm", # see below for valid values
"start_fixed" : True # i.e., DOF values at first timestep are fixed based on current robot state
},
"costs" : [
{
"type" : "joint_vel", # joint-space velocity cost
"params": {"coeffs" : [0]} # a list of length one is automatically expanded to a list of length n_dofs
# also valid: [1.9, 2, 3, 4, 5, 5, 4, 3, 2, 1]
},
{
"type" : "collision",
"params" : {
"coeffs" : [20], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
"dist_pen" : [0.25] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
},
}
],
"constraints" : [
{
"type" : "joint", # joint-space target
"params" : {"vals" : joint_target } # length of vals = # dofs of manip
},
{
"type" : "joint_vel_limits",
"name" : "joint_vel_limits",
"params" : {
"first_step" : 0,
"last_step" : n_steps-1, #inclusive
"vals" : [delta_t * max_joint_velocity for i in xrange(3)]
}
}
],
"init_info" : {
"type" : "straight_line", # straight line in joint space.
"endpoint" : joint_target
}
}
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, env) # create object that stores optimization problem
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob) # do optimization
t_elapsed = time.time() - t_start
print "result: " + str(result)
print "optimization took %.3f seconds"%t_elapsed
prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem
print traj_is_safe(result.GetTraj(), robot) # Check that trajectory is collision free
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())
if args.discrete: n_steps = 18
else: n_steps = 11
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" : "collision" if args.discrete else "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:
print "trying with midpoint", waypoint
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)
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()
prob.SetRobotActiveDOFs()
if not traj_is_safe(traj, robot):
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
def test():
rospy.init_node('test_IK',anonymous=True)
rp = RavenPlanner(Constants.Arm.Right)
rospy.sleep(2)
startJoints = {0:0.51091998815536499,
1:1.6072717905044558,
2:-0.049991328269243247,
4:0.14740140736103061,
5:0.10896652936935426,
8:-0.31163200736045837}
endJoints = {0:0.45221099211619786,
1:2.2917657932075581,
2:-0.068851854076958902,
4:0.44096283117933965,
5:0.32085205361054148,
8:-0.82079765727524379}
rp.updateOpenraveJoints(endJoints)
endEE = rp.manip.GetEndEffectorTransform()
rp.updateOpenraveJoints(startJoints)
startEE = rp.manip.GetEndEffectorTransform()
rp.env.SetViewer('qtcoin')
endJointPositions = []
for raveJointType in rp.manip.GetArmIndices():
rosJointType = rp.raveJointTypesToRos[raveJointType]
endJointPositions.append(endJoints[rosJointType])
n_steps = 15
endEEPose = tfx.pose(endEE)
desPos = endEEPose.position.list
endEEQuat = endEEPose.orientation
wxyzQuat = [endEEQuat.w, endEEQuat.x, endEEQuat.y, endEEQuat.z]
request = {
"basic_info" : {
"n_steps" : n_steps,
"manip" : rp.manip.GetName(),
"start_fixed" : True
},
"costs" : [
{
"type" : "joint_vel",
"params": {"coeffs" : [1]}
},
{
"type" : "collision",
"params" : {
"coeffs" : [100],
"continuous" : True,
"dist_pen" : [0.001]
}
},
],
"constraints" : [
{
"type" : "pose",
"name" : "target_pose",
"params" : {"xyz" : desPos,
"wxyz" : wxyzQuat,
"link" : rp.toolFrame,
"rot_coeffs" : [5,5,5],
"pos_coeffs" : [100,100,100]
}
}
],
"init_info" : {
"type" : "straight_line",
"endpoint" : endJointPositions
}
}
# convert dictionary into json-formatted string
s = json.dumps(request)
# create object that stores optimization problem
prob = trajoptpy.ConstructProblem(s, rp.env)
# do optimization
result = trajoptpy.OptimizeProblem(prob)
# check trajectory safety
from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs()
if not traj_is_safe(result.GetTraj(), rp.robot):
rospy.loginfo('Trajopt trajectory is not safe. Trajopt failed!')
rospy.loginfo('Still returning trajectory')
#return
j = rp.trajoptTrajToDicts(result.GetTraj())
Util.plot_transform(rp.env, startEE)
Util.plot_transform(rp.env, endEE)
IPython.embed()
def PlanToTSR(self, robot, tsrchains, is_interactive=False, **kw_args):
"""
Plan using the given list of TSR chains with TrajOpt.
@param robot the robot whose active manipulator will be used
@param tsrchains a list of TSRChain objects to respect during planning
@param is_interactive pause every iteration, until you press 'p'
or press escape to disable further plotting
@return traj
"""
import json
import time
import trajoptpy
manipulator = robot.GetActiveManipulator()
n_steps = 20
n_dofs = robot.GetActiveDOF()
# Create separate lists for the goal and trajectory-wide constraints.
goal_tsrs = [t for t in tsrchains if t.sample_goal]
traj_tsrs = [t for t in tsrchains if t.constrain]
# Find an initial collision-free IK solution by sampling goal TSRs.
from openravepy import (IkFilterOptions, IkParameterization,
IkParameterizationType)
for tsr in self._TsrSampler(goal_tsrs):
ik_param = IkParameterization(tsr.sample(),
IkParameterizationType.Transform6D)
init_joint_config = manipulator.FindIKSolutions(
ik_param, IkFilterOptions.CheckEnvCollisions)
if init_joint_config:
break
if not init_joint_config:
raise PlanningError('No collision-free IK solutions.')
# Construct a planning request with these constraints.
request = {
"basic_info": {
"n_steps": n_steps,
"manip": "active",
"start_fixed": True
},
"costs": [{
"type": "joint_vel",
"params": {
"coeffs": [1]
}
}, {
"type": "collision",
"params": {
"coeffs": [20],
"dist_pen": [0.025]
},
}],
"constraints": [],
"init_info": {
"type": "straight_line",
"endpoint": init_joint_config.tolist()
}
}
# Set up environment.
env = robot.GetEnv()
trajoptpy.SetInteractive(is_interactive)
# Convert dictionary into json-formatted string and create object that
# stores optimization problem.
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, env)
for t in xrange(1, n_steps):
prob.AddConstraint(constraints._TsrCostFn(robot, traj_tsrs),
[(t, j) for j in xrange(n_dofs)], "EQ",
"up{:d}".format(t))
prob.AddConstraint(constraints._TsrCostFn(robot, goal_tsrs),
[(n_steps - 1, j) for j in xrange(n_dofs)], "EQ",
"up{:d}".format(t))
# Perform trajectory optimization.
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob)
t_elapsed = time.time() - t_start
logger.info("Optimization took {:.3f} seconds".format(t_elapsed))
# Check for constraint violations.
if result.GetConstraints():
raise PlanningError(
"Trajectory did not satisfy constraints: {:s}".format(
str(result.GetConstraints())))
# Check for the returned trajectory.
waypoints = result.GetTraj()
if waypoints is None:
raise PlanningError("Trajectory result was empty.")
# Set active DOFs to match active manipulator and plan.
p = openravepy.KinBody.SaveParameters
with robot.CreateRobotStateSaver(p.ActiveDOF):
# Set robot DOFs to DOFs in optimization problem
prob.SetRobotActiveDOFs()
# Check that trajectory is collision free
from trajoptpy.check_traj import traj_is_safe
if not traj_is_safe(waypoints, robot):
return PlanningError("Result was in collision.")
# Convert the waypoints to a trajectory.
return self._WaypointsToTraj(robot, waypoints)
},
}
],
"constraints" : [
{
"type" : "joint",
"params" : {"vals" : joint_target}
}
],
"init_info" : {
"type" : "straight_line",
"endpoint" : joint_target
}
}
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, env)
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob)
t_elapsed = time.time() - t_start
print result
print "optimization took %.3f seconds"%t_elapsed
from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs()
assert traj_is_safe(result.GetTraj(), robot)
traj = result.GetTraj()
for t in traj:
robot.SetDOFValues(t, manip.GetArmIndices())
time.sleep(0.25)
}
s = json.dumps(request) # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(s, env) # create object that stores optimization problem
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob) # do optimization
t_elapsed = time.time() - t_start
print result
print "optimization took %.3f seconds"%t_elapsed
raw_input("Press enter to continue...")
from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem
#assert traj_is_safe(result.GetTraj(), robot) # Check that trajectory is collision free
print "Is traj safe:",traj_is_safe(result.GetTraj(), robot)
with env:
traj = RaveCreateTrajectory(env,'')
spec = IkParameterization.GetConfigurationSpecificationFromType(IkParameterizationType.Transform6D,'linear')
print robot.GetActiveConfigurationSpecification()
traj.Init(robot.GetActiveConfigurationSpecification());
for traj_pts in result.GetTraj():
print traj_pts
traj.Insert(traj.GetNumWaypoints(), traj_pts)
planningutils.RetimeActiveDOFTrajectory(traj,robot,hastimestamps=False,maxvelmult=0.01,maxaccelmult=0.01,plannername='LinearTrajectoryRetimer')
def plan(robot, manip, end_joints, end_pose = None):
arm_inds = robot.GetManipulator(manip).GetArmIndices()
start_joints = robot.GetDOFValues(arm_inds)
n_steps = 11
coll_coeff = 10
dist_pen = .05
waypoint_step = (n_steps - 1)// 2
joint_waypoints = [(np.asarray(start_joints) + np.asarray(end_joints))/2]
joint_waypoints.extend(get_postures(manip))
success = False
t_start = time()
t_verify = 0
t_opt = 0
for (i_init,waypoint) in enumerate(joint_waypoints):
d = {
"basic_info" : {
"n_steps" : n_steps,
"manip" : manip,
"start_fixed" : True
},
"costs" : [
{
"type" : "joint_vel",
"params": {"coeffs" : [1]}
},
{
"type" : "collision",
"params" : {"coeffs" : [coll_coeff],"dist_pen" : [dist_pen]}
},
{
"type" : "collision",
"params" : {"continuous": False, "coeffs" : [coll_coeff],"dist_pen" : [dist_pen]}
}
],
"constraints" : [
{"type" : "joint", "params" : {"vals" : end_joints.tolist()}} if end_pose is None else
{"type" : "pose", "params" : {"xyz" : end_pose[4:7].tolist(), "wxyz" : end_pose[0:4].tolist(), "link":"%s_gripper_tool_frame"%manip[0]}}
],
"init_info" : {
"type" : "given_traj"
}
}
if args.multi_init:
# print "trying with midpoint", waypoint
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)
d["init_info"]["data"] = [row.tolist() for row in inittraj]
s = json.dumps(d)
prob = trajoptpy.ConstructProblem(s, env)
t_start_opt = time()
result = trajoptpy.OptimizeProblem(prob)
t_opt += time() - t_start_opt
traj = result.GetTraj()
prob.SetRobotActiveDOFs()
t_start_verify = time()
is_safe = traj_is_safe(traj, robot)
t_verify += time() - t_start_verify
if not is_safe:
print "optimal trajectory has a collision. trying a new initialization"
else:
print "planning successful after %s initialization"%(i_init+1)
success = True
break
t_total = time() - t_start
return PlanResult(success, t_total, t_opt, t_verify)
t_elapsed = time.time() - t_start
print "optimization took %.3f seconds"%t_elapsed
traj = result.GetTraj()
# IPython.embed()
from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem
# for t in traj:
# print t
def f(x):
# print "origin" + str(x)
# print "new" + str(np.append(x, joint_start1))
return np.append(x, joint_start1)
robot_traj = map(f, traj)
# print robot_traj
print "[IS SAFE]: " + str(traj_is_safe(robot_traj, robot, 200)) # Check that trajectory is collision free
joint_angles = result.GetTraj()
# Visualize across all joint_angles
# for i in range(len(joint_angles)):
# robot.SetDOFValues(joint_angles[i], robot.GetManipulator('<arm name>').GetArmIndices()) # iterates across all joint angles
# time.sleep(1)
# IPython.embed()
env.SetViewer('qtcoin')
viewer = env.GetViewer()
viewer.SetBkgndColor([.8, .85, .9])
robot = env.GetRobots()[0]
joint_start1 = [3.14/3, 3.14/4, 0]
robot.SetDOFValues(joint_start1, robot.GetManipulator('left_arm').GetArmIndices())
print "optimization took %.3f seconds"%t_elapsed
traj = result.GetTraj()
<<<<<<< HEAD
# IPython.embed()
from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem
# for t in traj:
# print t
def f(x):
# print "origin" + str(x)
# print "new" + str(np.append(x, joint_start1))
return np.append(x, joint_start1)
robot_traj = map(f, traj)
# print robot_traj
print "[IS SAFE]: " + str(traj_is_safe(robot_traj, robot, 200)) # Check that trajectory is collision free
=======
# IPython.embed()
from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem
assert traj_is_safe(result.GetTraj(), robot) # Check that trajectory is collision free
>>>>>>> 0d7cac2c48fdfead3e63707083f26a33f2233e13
joint_angles = result.GetTraj()
# Visualize across all joint_angles
# for i in range(len(joint_angles)):
# robot.SetDOFValues(joint_angles[i], robot.GetManipulator('<arm name>').GetArmIndices()) # iterates across all joint angles
# time.sleep(1)
def __init__(self, interactive):
env = openravepy.Environment()
env.StopSimulation()
print "Loading robot model"
env.Load("model/model_test.xml")
print "loaded robot model"
print "Loading environment"
env.Load("environment/env.xml")
print "Environment loaded"
trajoptpy.SetInteractive(True) # pause every iteration, until you press 'p'. Press escape to disable further plotting
robot = env.GetRobots()[0]
print robot.GetManipulator("arm")
print robot.GetManipulator('arm').GetArmIndices()
joint_start = [0.0, 0.0]
robot.SetDOFValues(joint_start, robot.GetManipulator('arm').GetArmIndices())
joint_target = [-np.pi / 2.0, 0.0]
n_steps = 100
control_rate = 30.0
delta_t = 1.0 / control_rate
max_velocity = 2.0
request = {
"basic_info" : {
"n_steps" : n_steps,
"manip" : "arm", # see below for valid values
"start_fixed" : True # i.e., DOF values at first timestep are fixed based on current robot state
},
"costs" : [
{
"type" : "joint_vel", # joint-space velocity cost
"params": {"coeffs" : [1]} # a list of length one is automatically expanded to a list of length n_dofs
# also valid: [1.9, 2, 3, 4, 5, 5, 4, 3, 2, 1]
},
{
"type" : "collision",
"params" : {
"coeffs" : [20], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
"dist_pen" : [0.25] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
},
}
],
"constraints" : [
{
"type" : "joint", # joint-space target
"params" : {"vals" : joint_target } # length of vals = # dofs of manip
},
{
"type" : "joint_vel_limits",
"name" : "joint_vel_limits",
"params" : {
"vals" : [delta_t * max_velocity, delta_t * max_velocity],
"first_step" : 0,
"last_step" : n_steps-1, #inclusive
}
}
],
"init_info" : {
"type" : "straight_line", # straight line in joint space.
"endpoint" : joint_target
}
}
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, env) # create object that stores optimization problem
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob) # do optimization
t_elapsed = time.time() - t_start
print result
print "optimization took %.3f seconds"%t_elapsed
prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem
print traj_is_safe(result.GetTraj(), robot) # Check that trajectory is collision free
kwargs["floating_base"] = True
kwargs["basegoal"] = np.r_[target_base_pose[4:7], target_base_pose[0:4]]
# run chomp
msg = ''
t_start = time()
is_safe = False
traj = []
if args.multi_init:
for i_init, inittraj in enumerate(init_trajs):
t = kwargs["starttraj"] = array_to_traj(robot, inittraj)
try:
rave_traj = m_chomp.runchomp(**kwargs)
saver.Restore() # set active dofs to original (including affine), needed for traj_to_array
traj = traj_to_array(robot, rave_traj)
if traj_is_safe(traj, robot):
is_safe = True
msg = "planning successful after %s initialization"%(i_init+1)
break
except Exception, e:
msg = "CHOMP failed with exception: %s" % repr(e)
continue
else:
kwargs["adofgoal"] = target_dof_values
try:
rave_traj = m_chomp.runchomp(**kwargs)
saver.Restore() # set active dofs to original (including affine), needed for traj_to_array
traj = traj_to_array(robot, rave_traj)
is_safe = traj_is_safe(traj, robot)
except Exception, e:
msg = "CHOMP failed with exception: %s" % repr(e)
def __init__(self, interactive):
env = openravepy.Environment()
env.StopSimulation()
print "Loading robot model"
env.Load("model/model_test.xml")
print "loaded robot model"
print "Loading environment"
env.Load("environment/env.xml")
print "Environment loaded"
trajoptpy.SetInteractive(
True
) # pause every iteration, until you press 'p'. Press escape to disable further plotting
robot = env.GetRobots()[0]
print robot.GetManipulator("arm")
print robot.GetManipulator('arm').GetArmIndices()
joint_start = [0.0, 0.0]
robot.SetDOFValues(joint_start,
robot.GetManipulator('arm').GetArmIndices())
joint_target = [-np.pi / 2.0, 0.0]
n_steps = 100
control_rate = 30.0
delta_t = 1.0 / control_rate
max_velocity = 2.0
request = {
"basic_info": {
"n_steps": n_steps,
"manip": "arm", # see below for valid values
"start_fixed":
True # i.e., DOF values at first timestep are fixed based on current robot state
},
"costs": [
{
"type": "joint_vel", # joint-space velocity cost
"params": {
"coeffs": [1]
} # a list of length one is automatically expanded to a list of length n_dofs
# also valid: [1.9, 2, 3, 4, 5, 5, 4, 3, 2, 1]
},
{
"type": "collision",
"params": {
"coeffs": [
20
], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
"dist_pen": [
0.25
] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
},
}
],
"constraints": [
{
"type": "joint", # joint-space target
"params": {
"vals": joint_target
} # length of vals = # dofs of manip
},
{
"type": "joint_vel_limits",
"name": "joint_vel_limits",
"params": {
"vals":
[delta_t * max_velocity, delta_t * max_velocity],
"first_step": 0,
"last_step": n_steps - 1, #inclusive
}
}
],
"init_info": {
"type": "straight_line", # straight line in joint space.
"endpoint": joint_target
}
}
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(
s, env) # create object that stores optimization problem
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob) # do optimization
t_elapsed = time.time() - t_start
print result
print "optimization took %.3f seconds" % t_elapsed
prob.SetRobotActiveDOFs(
) # set robot DOFs to DOFs in optimization problem
print traj_is_safe(result.GetTraj(),
robot) # Check that trajectory is collision free
# Also valid: "coeffs" : [7,6,5,4,3,2,1]
},
{
"type" : "continuous_collision",
"name" :"cont_collision", # Shorten name so printed table will be prettier
"params" : {
"coeffs" : [20], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
"dist_pen" : [0.025] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
}
}
],
"constraints" : [
{
"type" : "joint", # joint-space target
"params" : {"vals" : joint_target } # length of vals = # dofs of manip
}
],
"init_info" : {
"type" : "straight_line", # straight line in joint space.
"endpoint" : joint_target
}
}
s = json.dumps(request) # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(s, env) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
print result
from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem
assert traj_is_safe(result.GetTraj(), robot) # Check that trajectory is collision free
def getTrajectoryFromPoseThread(self, once=False):
"""
Thread that uses trajopt to compute trajectory
Thread waits for all values in self.trajRequest
to be true
Sets the values of all the keys in self.poseTraj
"""
while not rospy.is_shutdown():
rospy.sleep(.05)
# block until all traj submissions received
while False in self.trajRequest.values():
if rospy.is_shutdown():
return
rospy.sleep(.05)
n_steps = 0
toolFrames = []
endPoses = []
endJointPositions = []
for armName in self.armNames:
n_steps += float(self.trajSteps[armName])/len(self.armNames)
toolFrames.append(self.toolFrame[armName])
endPose = self.trajEndPose[armName]
worldFromEE = tfx.pose(self.transformRelativePoseForIk(armName, endPose.matrix, endPose.frame, self.toolFrame[armName]))
endPoses.append(worldFromEE)
endJoints = self.trajEndJoints[armName]
# only keep shoulder, elbow, insertion, rotation, pitch, yaw
endJoints = dict([(jointType,jointPos) for jointType, jointPos in endJoints.items() if jointType in self.rosJointTypes])
for raveJointType in self.manip[armName].GetArmIndices():
rosJointType = self.raveJointTypesToRos[armName][raveJointType]
endJointPositions.append(endJoints[rosJointType])
self.updateOpenraveJoints(armName, self.trajStartJoints[armName])
n_steps = int(n_steps)
if self.trajReqType == Request.Type.Joints:
request = Request.joints(n_steps, endJointPositions)
else:
request = Request.pose(n_steps, endJointPositions, toolFrames, endPoses)
#IPython.embed()
# convert dictionary into json-formatted string
s = json.dumps(request)
#print s
#print self.robot.GetActiveDOFValues()
#raise Exception()
# create object that stores optimization problem
prob = trajoptpy.ConstructProblem(s, self.env)
#raise Exception()
# do optimization
result = trajoptpy.OptimizeProblem(prob)
# check trajectory safety
from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs()
if not traj_is_safe(result.GetTraj(), self.robot):
rospy.loginfo('Trajopt trajectory is not safe. Trajopt failed!')
for armName in self.armNames:
self.poseTraj[armName] = None
else:
startIndex = 0
for armName in self.armNames:
endIndex = startIndex + len(self.manipJoints[armName])
armJointTrajArray = result.GetTraj()[:,startIndex:endIndex]
jointTrajDicts = self.jointTrajToDicts(armName, armJointTrajArray)
self.jointTraj[armName] = jointTrajDicts # for debugging
poseTraj = self.jointDictsToPoses(armName, jointTrajDicts)
self.poseTraj[armName] = poseTraj
startIndex = endIndex
#print self.jointTraj
#raise Exception()
for armName in self.armNames:
self.trajRequest[armName] = False
if once:
break
def plan(robot, manip, end_joints, end_pose=None):
arm_inds = robot.GetManipulator(manip).GetArmIndices()
start_joints = robot.GetDOFValues(arm_inds)
n_steps = 11
coll_coeff = 10
dist_pen = .05
waypoint_step = (n_steps - 1) // 2
joint_waypoints = [(np.asarray(start_joints) + np.asarray(end_joints)) / 2]
joint_waypoints.extend(get_postures(manip))
success = False
t_start = time()
t_verify = 0
t_opt = 0
for (i_init, waypoint) in enumerate(joint_waypoints):
d = {
"basic_info": {
"n_steps": n_steps,
"manip": manip,
"start_fixed": True
},
"costs": [{
"type": "joint_vel",
"params": {
"coeffs": [1]
}
}, {
"type": "collision",
"params": {
"coeffs": [coll_coeff],
"dist_pen": [dist_pen]
}
}, {
"type": "collision",
"params": {
"continuous": False,
"coeffs": [coll_coeff],
"dist_pen": [dist_pen]
}
}],
"constraints": [{
"type": "joint",
"params": {
"vals": end_joints.tolist()
}
} if end_pose is None else {
"type": "pose",
"params": {
"xyz": end_pose[4:7].tolist(),
"wxyz": end_pose[0:4].tolist(),
"link": "%s_gripper_tool_frame" % manip[0]
}
}],
"init_info": {
"type": "given_traj"
}
}
if args.multi_init:
# print "trying with midpoint", waypoint
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)
d["init_info"]["data"] = [row.tolist() for row in inittraj]
s = json.dumps(d)
prob = trajoptpy.ConstructProblem(s, env)
t_start_opt = time()
result = trajoptpy.OptimizeProblem(prob)
t_opt += time() - t_start_opt
traj = result.GetTraj()
prob.SetRobotActiveDOFs()
t_start_verify = time()
is_safe = traj_is_safe(traj, robot)
t_verify += time() - t_start_verify
if not is_safe:
print "optimal trajectory has a collision. trying a new initialization"
else:
print "planning successful after %s initialization" % (i_init + 1)
success = True
break
t_total = time() - t_start
return PlanResult(success, t_total, t_opt, t_verify)
def _Plan(self, robot, request,
traj_constraints=(), goal_constraints=(),
traj_costs=(), goal_costs=(),
interactive=False, constraint_threshold=1e-4,
**kwargs):
"""
Plan to a desired configuration with Trajopt.
This function invokes the Trajopt planner directly on the specified
JSON request. This can be used to implement custom path optimization
algorithms.
Constraints and costs are specified as dicts of:
```
{
'f': [float] -> [float],
'dfdx': [float] -> [float],
'type': ConstraintType or CostType
'dofs': [int]
}
```
The input to f(x) and dfdx(x) is a vector of active DOF values used in
the planning problem. The output is a vector of costs, where the
value *increases* as a constraint or a cost function is violated or
unsatisfied.
See ConstraintType and CostType for descriptions of the various
function specifications and their expected behavior.
The `dofs` parameter can be used to specify a subset of the robot's
DOF indices that should be used. A ValueError is thrown if these
indices are not entirely contained in the current active DOFs of the
robot.
@param robot: the robot whose active DOFs will be used
@param request: a JSON planning request for Trajopt
@param traj_constraints: list of dicts of constraints that should be
applied over the whole trajectory
@param goal_constraints: list of dicts of constraints that should be
applied only at the last waypoint
@param traj_costs: list of dicts of costs that should be applied over
the whole trajectory
@param goal_costs: list of dicts of costs that should be applied only
at the last waypoint
@param interactive: pause every iteration, until you press 'p' or press
escape to disable further plotting
@param constraint_threshold: acceptable per-constraint violation error
@returns traj: trajectory from current configuration to specified goal
"""
import json
import trajoptpy
# Set up environment.
env = robot.GetEnv()
trajoptpy.SetInteractive(interactive)
# Trajopt's UserData gets confused if the same environment
# is cloned into multiple times, so create a scope to later
# remove all TrajOpt UserData keys.
try:
# Convert dictionary into json-formatted string and create object
# that stores optimization problem.
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, env)
assert(request['basic_info']['manip'] == 'active')
assert(request['basic_info']['n_steps'] is not None)
n_steps = request['basic_info']['n_steps']
n_dofs = robot.GetActiveDOF()
i_dofs = robot.GetActiveDOFIndices()
# Add trajectory-wide costs and constraints to each timestep.
for t in xrange(1, n_steps):
for constraint in traj_constraints:
self._addFunction(prob, t, i_dofs, n_dofs, constraint)
for cost in traj_costs:
self._addFunction(prob, t, i_dofs, n_dofs, cost)
# Add goal costs and constraints.
for constraint in goal_constraints:
self._addFunction(prob, n_steps-1, i_dofs, n_dofs, constraint)
for cost in goal_costs:
self._addFunction(prob, n_steps-1, i_dofs, n_dofs, cost)
# Perform trajectory optimization.
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob)
t_elapsed = time.time() - t_start
logger.debug("Optimization took {:.3f} seconds".format(t_elapsed))
# Check for constraint violations.
for name, error in result.GetConstraints():
if error > constraint_threshold:
raise PlanningError(
"Trajectory violates contraint '{:s}': {:f} > {:f}"
.format(name, error, constraint_threshold)
)
# Check for the returned trajectory.
waypoints = result.GetTraj()
if waypoints is None:
raise PlanningError("Trajectory result was empty.")
# Set active DOFs to match active manipulator and plan.
p = openravepy.KinBody.SaveParameters
with robot.CreateRobotStateSaver(p.ActiveDOF):
# Set robot DOFs to DOFs in optimization problem
prob.SetRobotActiveDOFs()
# Check that trajectory is collision free
from trajoptpy.check_traj import traj_is_safe
if not traj_is_safe(waypoints, robot):
raise PlanningError("Result was in collision.")
# Convert the waypoints to a trajectory.
return self._WaypointsToTraj(robot, waypoints)
finally:
for body in env.GetBodies():
for key in TRAJOPT_USERDATA_KEYS:
body.RemoveUserData(key)
trajopt_env_userdata = env.GetKinBody('__trajopt_data__')
if trajopt_env_userdata is not None:
env.Remove(trajopt_env_userdata)
# IPython.embed()
from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem
# for t in traj:
# print t
def f(x):
# print "origin" + str(x)
# print "new" + str(np.append(x, joint_start1))
return np.append(x, joint_start1)
robot_traj = map(f, traj)
# print robot_traj
print "[IS SAFE]: " + str(traj_is_safe(
robot_traj, robot, 200)) # Check that trajectory is collision free
joint_angles = result.GetTraj()
# Visualize across all joint_angles
# for i in range(len(joint_angles)):
# robot.SetDOFValues(joint_angles[i], robot.GetManipulator('<arm name>').GetArmIndices()) # iterates across all joint angles
# time.sleep(1)
# IPython.embed()
env.SetViewer('qtcoin')
viewer = env.GetViewer()
viewer.SetBkgndColor([.8, .85, .9])
robot = env.GetRobots()[0]
joint_start1 = [3.14 / 3, 3.14 / 4, 0]
robot.SetDOFValues(joint_start1,
# run chomp
msg = ''
t_start = time()
status = PlannerStatus.FAIL_COLLISION
traj = []
if args.multi_init:
for i_init, inittraj in enumerate(init_trajs):
t = kwargs["starttraj"] = array_to_traj(robot, inittraj)
try:
rave_traj = m_chomp.runchomp(**kwargs)
saver.Restore(
) # set active dofs to original (including affine), needed for traj_to_array
traj = traj_to_array(robot, rave_traj)
if not traj_in_joint_limits(traj, robot):
status = PlannerStatus.FAIL_JOINT_VIOLATION
elif not traj_is_safe(traj, robot):
status = PlannerStatus.FAIL_COLLISION
else:
status = PlannerStatus.SUCCESS
msg = "planning successful after %s initialization" % (
i_init + 1)
break
except Exception, e:
msg = "CHOMP failed with exception: %s" % str(e)
if 'limit' in msg:
status = PlannerStatus.FAIL_JOINT_VIOLATION
else:
status = PlannerStatus.FAIL_OTHER
continue
else:
kwargs["adofgoal"] = target_dof_values
