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 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