def TerminateMove(): """ Function defining the termination condition. Fail if deviation larger than position and angular tolerance. Succeed if distance moved is larger than max_distance. Cache and continue if distance moved is larger than distance. """ from .exceptions import ConstraintViolationPlanningError T_ee_curr = manip.GetEndEffectorTransform() # Find where we are on the goal trajectory by finding # the the closest point (_, t, _) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj( T_ee_curr, traj, 0.0005) # Get the desired end-effector transform from # the goal trajectory desired_T_ee = openravepy.matrixFromPose(traj.Sample(t)[0:7]) # Get the position vector tangent to the trajectory, # using finite-differences pose_ee_next = traj.Sample(t + t_step)[0:7] desired_T_ee_next = openravepy.matrixFromPose(pose_ee_next) tangent_vec = desired_T_ee_next[0:3, 3] - desired_T_ee[0:3, 3] # Calculate error between current end-effector pose # and where we should be on the goal trajectory geodesic_error = util.GeodesicError(desired_T_ee, T_ee_curr) orientation_error = geodesic_error[3] position_error_vec = geodesic_error[0:3] # Use only the translation error that is perpendicular # to our current position tangent_trans_error = \ position_error_vec - numpy.dot( position_error_vec, util.NormalizeVector(tangent_vec)) tangent_trans_error = numpy.nan_to_num(tangent_trans_error) position_error = tangent_trans_error if numpy.fabs(orientation_error) > angular_tolerance: raise ConstraintViolationPlanningError( 'Deviated from orientation constraint.') position_deviation = numpy.linalg.norm(position_error) if position_deviation > position_tolerance: raise ConstraintViolationPlanningError( 'Deviated from straight line constraint.') # Check if we have reached the end of the goal trajectory error_to_goal = util.GeodesicError(T_ee_curr, T_ee_goal) orientation_error = error_to_goal[3] # radians position_error = error_to_goal[0:3] # x,y,z if ((numpy.fabs(orientation_error) < angular_tolerance) and (numpy.linalg.norm(position_error) < position_tolerance)): return Status.CACHE_AND_TERMINATE return Status.CONTINUE
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 = not check_traj(traj, manip, 100), traj = traj, planning_time = mpr.planning_time.to_sec(), error_code = mpr.error_code.val)
def vf_path(): """ Function defining a joint-space vector field. """ T_ee_actual = manip.GetEndEffectorTransform() # Find where we are on the goal trajectory by finding # the the closest point (_, t, _) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj( T_ee_actual, traj, 0.0005) # Get the desired end-effector transform from # the goal trajectory desired_T_ee = openravepy.matrixFromPose(traj.Sample(t)[0:7]) # Get the next end-effector transform, using finite-differences pose_ee_next = traj.Sample(t + t_step)[0:7] desired_T_ee_next = openravepy.matrixFromPose(pose_ee_next) # Get the translation tangent to current position tangent_vec = desired_T_ee_next[0:3, 3] - desired_T_ee[0:3, 3] # Get the translational error position_error_vec = desired_T_ee[0:3, 3] - T_ee_actual[0:3, 3] # Get the translational error perpendicular to the path tangent_trans_error = \ position_error_vec - numpy.dot( position_error_vec, util.NormalizeVector(tangent_vec)) tangent_trans_error = numpy.nan_to_num(tangent_trans_error) # The twist between the actual end-effector position and # where it should be on the goal trajectory # (the error term) twist_perpendicular = util.GeodesicTwist(T_ee_actual, desired_T_ee) twist_perpendicular[0:3] = tangent_trans_error # The twist tangent to where the end-effector should be # on the goal trajectory # (the feed-forward term) twist_parallel = util.GeodesicTwist(desired_T_ee, desired_T_ee_next) # Normalize the translational and angular velocity of # the feed-forward twist twist_parallel[0:3] = util.NormalizeVector(twist_parallel[0:3]) twist_parallel[3:6] = util.NormalizeVector(twist_parallel[3:6]) # Apply gains twist = Kp_e * twist_perpendicular + Kp_ff * twist_parallel # Calculate joint velocities using an optimized jacobian dqout, _ = util.ComputeJointVelocityFromTwist( robot, twist, joint_velocity_limits=numpy.PINF) return dqout
def warehouse_scene_pairwise_test(initial_state_id, goal_constraint_starts_with, robot, arm="right", planner_id=""): from warehouse_utils import MoveitWarehouseDatabase constraints_db = MoveitWarehouseDatabase('moveit_constraints') states_db = MoveitWarehouseDatabase('moveit_robot_states') initial_state = states_db.get_message(RobotState, state_id=initial_state_id) query = {'constraints_id':{"$regex":"^%s.*" % goal_constraint_starts_with}} constraints = constraints_db.get_messages(Constraints, query) print "Got", len(constraints), "goal constraints from the warehouse" initial_states = [initial_state] positions, quats = [], [] results = [] 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) update_rave_from_ros(robot, initial_state.joint_state.position, initial_state.joint_state.name) for c in constraints: pos, quat = pose_constraints_to_xyzquat(c) post = (pos.x, pos.y, pos.z) quatt = (quat.x, quat.y, quat.z, quat.w) positions.append(post) quats.append(quatt) state = robot_state_from_pose_goal(post, quatt, arm, robot, initial_state) if state: initial_states.append(state) for start_state in initial_states: update_rave_from_ros(robot, start_state.joint_state.position, start_state.joint_state.name) for posit, quater in zip(positions, quats): result = test_plan_to_pose(posit, quater, arm, robot, start_state, planner_id, "%s_wrist_roll_link") if result is not None: results.append(result) process_results(results)
def handle_conf_generator(req): # request gp = req.grasp_poses[0] m_gp = openravepy.matrixFromPose((gp.orientation.w, gp.orientation.x, gp.orientation.y, gp.orientation.z,\ gp.position.x, gp.position.y, gp.position.z)) bps = cg._find_base_conf(m_gp, req.base_num) # response pcr = PossibleConfResponse() i = 0 if bps is not None: for b in bps[0]: axisAngle = openravepy.axisAngleFromQuat(b[0:4]) bm = [b[4], b[5], axisAngle[2]] bc = BaseConf() bc.b_c.append(bm[0]) bc.b_c.append(bm[1]) bc.b_c.append(bm[2]) pcr.base_confs.append(bc) jc = JointConf() if (i < req.joint_num): robot.SetActiveDOFValues(bm) joint_conf = manip.FindIKSolution( m_gp, filteroptions=openravepy.IkFilterOptions.CheckEnvCollisions ) if joint_conf is not None: jc.j_c = joint_conf i = i + 1 pcr.joint_confs.append(jc) return pcr
def build_joint_request(jvals, arm, robot, initial_state=build_robot_state(), planner_id=''): m = MotionPlanRequest() m.group_name = "%s_arm" % arm m.start_state = initial_state m.planner_id = planner_id c = Constraints() joints = robot.GetJoints() joint_inds = robot.GetManipulator("%sarm" % arm).GetArmIndices() c.joint_constraints = [ JointConstraint(joint_name=joints[joint_inds[i]].GetName(), position=jvals[i]) for i in xrange(len(jvals)) ] jc = JointConstraint() m.goal_constraints = [c] m.allowed_planning_time = rospy.Duration(args.max_planning_time) 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) return m
def test_position_base(pose_targ): wxyz_targ = pose_targ[:4] xyz_targ = pose_targ[4:] angle_init = np.random.rand() * 2*np.pi x_init = xyz_targ[0] - .5*np.cos(angle_init) y_init = xyz_targ[1] - .5*np.sin(angle_init) dofvals_init = np.r_[np.zeros(7), 0, x_init, y_init, angle_init] robot.SetDOFValues([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) robot.SetTransform(rave.matrixFromPose([1, 0, 0, 0, -3.4, -1.4, 0.05])) manipname = "rightarm+torso_lift_joint+base" n_steps = 1 prob_desc = make_request_skeleton(manipname, n_steps); prob_desc["basic_info"]["start_fixed"] = False prob_desc["costs"].append( {"type" : "pose", "name" : "pose", "params" : { "pos_coeffs" : [10,10,10], "rot_coeffs" : [0,0,0], "xyz" : xyz_targ, "wxyz" : wxyz_targ, "link" : "r_gripper_tool_frame", }}) prob_desc["init_info"]["type"] = "given_traj" prob_desc["init_info"]["data"] = [dofvals_init.tolist()] s = json.dumps(prob_desc) traj.SetInteractive(True); prob = traj.ConstructProblem(s, env) return traj.OptimizeProblem(prob)
def test_ikfast_6d_case_1(self): """ Easy test. IKFast 6D """ i = 0 for (qseed, pose) in zip(self.qseeds, self.poses): i += 1 T = orpy.matrixFromPose(pose) with self.robot: self.robot.SetActiveDOFValues(qseed) ts = time.time() sol = self.manip.FindIKSolution(T, ikfilter_checkcollision) te = time.time() if sol is not None: self.total_time += te - ts self.no_success += 1 with self.robot: self.robot.SetActiveDOFValues(sol) pose_actual = self.manip.GetTransformPose() if pose_actual[0] < 0: pose_actual[:4] *= -1. np.testing.assert_allclose(pose_actual, pose, rtol=1e-5, atol=1e-5) self.assertTrue((sol <= self.q_max).all(), msg="Violate joint limits") self.assertTrue((self.q_min <= sol).all(), msg="Violate joint limits")
def SampleWorkspaceTrajectoryIntoArray(robot, traj, t_start, t_end, n): """ Takes n samples from a workspace trajectory, including the end-points, and returns them in an array. """ if t_start < 0.0: raise ValueError("t_start < 0.0 " "in SampleWorkspaceTrajectoryIntoArray()") if t_end <= t_start: raise ValueError("t_end <= t_start " "in SampleWorkspaceTrajectoryIntoArray()") num_samples = n if num_samples <= 3: raise ValueError("Number of samples 'n' must be >= 3 " "in SampleWorkspaceTrajectoryIntoArray()") cspec = traj.GetConfigurationSpecification() dof_indices, _ = cspec.ExtractUsedIndices(robot) if t_end == None: t_end = traj.GetDuration() times_list = [] transforms_list = [] for t in numpy.linspace(t_start, t_end, num_samples): times_list.append(t) P = traj.Sample(t)[0:7] # first 7 values are pose T_current = openravepy.matrixFromPose(P) transforms_list.append(T_current) times_array = numpy.asarray(times_list) transforms_array = numpy.asarray(transforms_list) return times_array,transforms_array
def create_polyhedron(self, contacts): self.handle = None self.nr_iter = 0 try: self.polyhedron = stabilipy.StabilityPolygon( robot.mass, dimension=3, radius=1.5) stabilipy_contacts = [] for contact in contacts: hmatrix = matrixFromPose(contact.pose) X, Y = contact.shape displacements = [array([[X, Y, 0]]).T, array([[-X, Y, 0]]).T, array([[-X, -Y, 0]]).T, array([[X, -Y, 0]]).T] for disp in displacements: stabilipy_contacts.append( stabilipy.Contact( contact.friction, hmatrix[:3, 3:] + hmatrix[:3, :3].dot(disp), hmatrix[:3, 2:3])) self.polyhedron.contacts = stabilipy_contacts self.polyhedron.select_solver(self.method) self.polyhedron.make_problem() self.polyhedron.init_algo() self.polyhedron.build_polys() vertices = self.polyhedron.polyhedron() self.handle = draw_polytope( [(x[0], x[1], x[2]) for x in vertices]) except Exception as e: print("SupportPolyhedronDrawer: {}".format(e))
def get_random_pose(env, limits, manip, frame_name="r_gripper_tool_frame"): robot = env.GetRobots()[0] is_success = False while is_success is not True: #Select which input space index = np.random.choice(limits.keys()) x, y, z = generate_random_xyz(limits[index]) #simple check for collision with a box if check_col_with_box(env, x, y, z): continue #do IK here xyz = [x, y, z] quat = [1, 0, 0, 0] # wxyz pose = openravepy.matrixFromPose(np.r_[quat, xyz]) joint_angle = ku.ik_for_link( pose, manip, frame_name, filter_options=openravepy.IkFilterOptions.CheckEnvCollisions) if joint_angle is None: continue #further check for collision robot.SetDOFValues(joint_angle, manip.GetArmIndices()) if env.CheckCollision(robot) or robot.CheckSelfCollision(): continue is_success = True xyz = np.array([x, y, z]) return joint_angle, xyz
def PublishWorkspaceTrajectoryPoses(rviz_tools, robot, traj, n=30, axis_length=0.02, axis_radius=0.001): """ Sample a workspace trajectory n number of times and publish an axes Rviz Marker representing the end effector pose at each sampled position. @param instance rviz_tools: An instance of the rviz_tools.RvizMarkers class @param openravepy.robot robot: The robot. @param openravepy.Trajectory traj: A timed workspace trajectory. @param n int: Number of axes to draw along the path of the trajectory, including end-points. @param axis_length float: The length of each of the 3 axis lines. @param axis_radius float: The radius of each of the 3 axis lines. """ if n <= 0: raise ValueError("Number of samples n must be a positive number" " in PublishWorkspaceTrajectoryPoses()") if not prpy.util.IsTimedTrajectory(traj): raise ValueError( 'Trajectory must be timed. If you want to use this function on a' ' path, then consider using util.ComputeUnitTiming to compute its' ' arclength parameterization.') # Draw specified number of axis markers along # the path of the trajectory duration = traj.GetDuration() for t in numpy.linspace(0, duration, n): P = traj.Sample(t)[0:7] # first 7 values are pose T_ee_curr = openravepy.matrixFromPose(P) duration = None # None = publish forever rviz_tools.publishAxis(T_ee_curr, axis_length, axis_radius, duration)
def find_body_holes(body, radius, absolute=True): import trimesh mesh_holes = dict() body_holes = dict() Tbody = body.GetTransform() for link in body.GetLinks(): link_holes = [] Tlink = link.GetTransform() for geometry in link.GetGeometries(): if geometry.GetType() == orpy.GeometryType.Trimesh: filename = geometry.GetRenderFilename() scale = geometry.GetRenderScale() pose = geometry.GetTransformPose() if filename not in mesh_holes: mesh = trimesh.load(filename) mesh_holes[filename] = find_mesh_holes( mesh.vertices, mesh.faces, radius, scale) for h in mesh_holes[filename]: hole = copy.deepcopy(h) Tmesh = np.dot(Tlink, orpy.matrixFromPose(pose)) if absolute: hole.transform(np.dot(Tbody, Tmesh)) else: hole.transform(Tmesh) link_holes.append(hole) if len(link_holes) > 0: body_holes[str(link.GetName())] = link_holes return body_holes
def move_to_pregrasp(self, grasp_pose, eps=1e-2): """ Move the robot to the pregrasp pose given by the grasp object """ # get grasp pose gripper_position = grasp_pose.position gripper_orientation = grasp_pose.orientation gripper_pose = np.array([ gripper_orientation.w, gripper_orientation.x, gripper_orientation.y, gripper_orientation.z, gripper_position.x, gripper_position.y, gripper_position.z ]) if abs(np.linalg.norm(np.array(gripper_orientation.list)) - 1.0) > eps: logging.warning('Illegal pose') return None, None # get grasp pose relative to object T_gripper_obj = rave.matrixFromPose(gripper_pose) T_obj_world = self.T_gripper_world_.dot(self.T_rviz_or_).dot( np.linalg.inv(T_gripper_obj)) # set robot position as inverse of object (for viewing purposes) T_robot_world = np.linalg.inv(T_obj_world) self.robot.SetTransform(T_robot_world) return T_gripper_obj, T_robot_world
def test_cart_traj(): robot.SetDOFValues([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) robot.SetTransform(rave.matrixFromPose([1, 0, 0, 0, 2, 0, 0])) manipname = "rightarm" manip = robot.GetManipulator(manipname) n_steps = 5 prob_desc = make_request_skeleton(manipname, n_steps); prob_desc["basic_info"]["start_fixed"] = False decimation = 1 pts = mu.linspace2d([2.5,-.3,1],[2.5,.3,1],n_steps/decimation) for (i,pt) in enumerate(pts): prob_desc["costs"].append( {"type" : "pose", "name" : "posexxx", "params" : { "pos_coeffs" : [1,1,1], "rot_coeffs" : [0,0,0], "xyz" : pt.tolist(), "wxyz" : [.8,.6,0,0], "link" : "r_gripper_tool_frame", "timestep" : i*decimation }} ) prob_desc["init_info"]["type"] = "stationary" s = json.dumps(prob_desc) traj.SetInteractive(True); prob = traj.ConstructProblem(s, env) return traj.OptimizeProblem(prob)
def _tf_to_rave_mat(tf): """ Convert a RigidTransform to an OpenRAVE matrix """ position = tf.position orientation = tf.quaternion pose = np.array([orientation[0], orientation[1], orientation[2], orientation[3], position[0], position[1], position[2]]) mat = rave.matrixFromPose(pose) return mat
def get_lin_interp_poses(start_pos, end_pos, n_steps): xyz_start, quat_start = juc.hmat_to_trans_rot(start_pos) xyz_end, quat_end = juc.hmat_to_trans_rot(end_pos) xyzs = math_utils.linspace2d(xyz_start, xyz_end, n_steps) quats = [rave.quatSlerp(quat_start, quat_end, t) for t in np.linspace(0, 1, n_steps)] hmats = [rave.matrixFromPose(np.r_[quat[3], quat[:3], xyz]) for (xyz, quat) in zip(xyzs, quats)] gripper_poses = np.array([juc.hmat_to_pose(hmat) for hmat in hmats]) return gripper_poses
def calcIKForPQ(self, pos, quat, opts=openravepy.IkFilterOptions.CheckEnvCollisions): '''TODO''' targetMat = openravepy.matrixFromPose(numpy.r_[quat, pos]) solutions = self.manip.FindIKSolutions(targetMat, opts) return solutions
def main(): import yaml import os import os.path as osp import sys sys.path.insert(1, os.path.join(sys.path[0], '..')) import planning_benchmark_common as pbc import argparse parser = argparse.ArgumentParser() parser.add_argument("problemfile", type=argparse.FileType("r")) parser.add_argument("--base_only", action="store_true") args = parser.parse_args() problemset = yaml.load(args.problemfile) assert problemset["active_affine"] == 11 env = openravepy.Environment() env.StopSimulation() env.Load(osp.join(pbc.envfile_dir, problemset["env_file"])) robot2file = {"pr2": "robots/pr2-beta-static.zae"} env.Load(robot2file[problemset["robot_name"]]) robot = env.GetRobots()[0] robot.SetTransform( openravepy.matrixFromPose(problemset["default_base_pose"])) rave_joint_names = [joint.GetName() for joint in robot.GetJoints()] rave_inds, rave_values = [], [] for (name, val) in zip(problemset["joint_names"], problemset["default_joint_values"]): if name in rave_joint_names: i = rave_joint_names.index(name) rave_inds.append(i) rave_values.append(val) robot.SetDOFValues(rave_values, rave_inds) active_joint_inds = [ rave_joint_names.index(name) for name in problemset["active_joints"] ] robot.SetActiveDOFs(active_joint_inds, problemset["active_affine"]) env.SetViewer('qtcoin') env.UpdatePublishedBodies() if args.base_only: poses = sample_base_positions(robot, num=100) else: poses = sample_all(robot, num=100) while True: for i, p in enumerate(poses): robot.SetActiveDOFValues(poses[i]) env.UpdatePublishedBodies() raw_input('showing %d/%d: joints = %s' % (i + 1, len(poses), ', '.join(str(v) for v in poses[i]))) print '=================='
def base_pose_to_mat(pose): # x, y, rot = pose assert len(pose) == 3 x = pose[0] y = pose[1] rot = pose[2] q = quatFromAxisAngle((0, 0, rot)).tolist() pos = [x, y, 0] # pos = np.vstack((x,y,np.zeros(1))) matrix = matrixFromPose(q + pos) return matrix
def _init_robot(self): """ Initialize the robot """ # set initial pose self.robot.SetTransform(rave.matrixFromPose(np.array([1,0,0,0,0,0,0]))) self.robot.SetDOFValues([0.54,-1.57, 1.57, 0.54],[22,27,15,34]) # get robot manipulation tools self.manip_ = self.robot.SetActiveManipulator("leftarm_torso") self.maniprob_ = rave.interfaces.BaseManipulation(self.robot) # create the interface for task manipulation programs self.taskprob_ = rave.interfaces.TaskManipulation(self.robot) # create the interface for task manipulation programs self.finger_joint_ = self.robot.GetJoint('l_gripper_l_finger_joint')
def set_pose(self, pose): """ Set the 7D pose of the body orientation. Parameters ---------- pose : (7,) array Pose of the body, i.e. quaternion + position in world frame. """ T = openravepy.matrixFromPose(pose) self.set_transform(T)
def add_drop_table(env, dist_from_robot): thickness = 0.1 legheight = 0.4 table = object_models.create_table(env, "table", 0.5, 0.5, thickness, 0.1, 0.1, legheight) x, y = dist_from_robot z = thickness / 2 + legheight rot = openravepy.quatFromAxisAngle((0, 0, 0)) table.SetTransform(openravepy.matrixFromPose(list(rot) + [x, y, z])) env.AddKinBody(table) return table
def robot_state_from_pose_goal(xyz, xyzw, arm, robot, initial_state = build_robot_state()): manip = robot.GetManipulator(arm + "arm") joint_solutions = ku.ik_for_link(rave.matrixFromPose(np.r_[xyzw[3], xyzw[:3], xyz]), manip, "%s_wrist_roll_link"%arm[0], 1, True) if len(joint_solutions) == 0: raise Exception joints = robot.GetJoints() joint_inds = robot.GetManipulator("%sarm"%arm).GetArmIndices() joint_names = [joints[joint_inds[i]].GetName() for i in xrange(len(joint_solutions[0]))] joint_values = [joint_solutions[0][i] for i in xrange(len(joint_solutions[0]))] new_state = alter_robot_state(initial_state, joint_names, joint_values) return new_state
def _init_openrave(self, viewer=False, envFile=None, pw_file=False): if type(envFile) == str: self.env = openravepy.Environment() self.env.StopSimulation() if viewer: self.env.SetViewer('qtcoin') self.env.Load(envFile) else: self.env = envFile fname = "pw_file.txt" if pw_file: if os.path.isfile(fname): with open(fname, "r") as f: transforms = eval(f.read()) for b in self.env.GetBodies(): if b.GetName() in transforms.keys(): b.SetTransform(openravepy.matrixFromPose(eval(transforms[b.GetName()]))) else: print("Could not find pw_file.txt, keeping transforms as is") robot = self.env.GetRobots()[0] # EnvManager.init_openrave_state(self.env) # PR2 robot intialization # # loading the IK models # robot.SetActiveManipulator('leftarm') # ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( # robot, iktype=openravepy.IkParameterization.Type.Transform6D) # if not ikmodel.load(): # ikmodel.autogenerate() # robot.SetActiveManipulator('rightarm') # ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( # robot, iktype=openravepy.IkParameterization.Type.Transform6D) # if not ikmodel.load(): # ikmodel.autogenerate() # # utils.set_rave_limits_to_soft_joint_limits(robot) # # start arms on the side # left_joints = Arm.L_POSTURES['side2'] # right_joints = mirror_arm_joints(left_joints).tolist() # # robot.SetDOFValues( # # right_joints + left_joints, # # robot.GetManipulator("rightarm").GetArmIndices().tolist() + # # robot.GetManipulator("leftarm").GetArmIndices().tolist()) # # start torso up # v = robot.GetActiveDOFValues() # v[robot.GetJoint('torso_lift_joint').GetDOFIndex()] = 0.3 # # robot.SetDOFValues(v) time.sleep(1)
def get_return_from_grasp_joint_trajectory(self, start_joints, target_pose, n_steps=40): """ Calls trajopt to plan return from grasp collision-free trajectory :param start_joints: list of initial joints :param target_pose: desired pose of tool_frame (tfx.pose) :param n_steps: trajopt discretization :return None if traj not collision free, else list of joint values """ assert len(start_joints) == len(self.joint_indices) assert target_pose.frame.count('base_link') == 1 # set active manipulator and start joint positions self.robot.SetDOFValues(start_joints, self.joint_indices) # initialize trajopt inputs rave_pose = tfx.pose(self.sim.transform_from_to(target_pose.matrix, target_pose.frame, 'world')) quat = rave_pose.orientation xyz = rave_pose.position quat_target = [quat.w, quat.x, quat.y, quat.z] xyz_target = [xyz.x, xyz.y, xyz.z] rave_mat = rave.matrixFromPose(np.r_[quat_target, xyz_target]) request = self._get_return_from_grasp_trajopt_request(xyz_target, quat_target, n_steps) # convert dictionary into json-formatted string s = json.dumps(request) # create object that stores optimization problem prob = trajoptpy.ConstructProblem(s, self.sim.env) tool_link = self.robot.GetLink(self.tool_frame) def penalize_low_height(x): self.robot.SetDOFValues(x, self.joint_indices, False) z = tool_link.GetTransform()[2,3] return max(0, 10.0 - z) for t in xrange(n_steps-2): prob.AddErrorCost(penalize_low_height, [(t,j) for j in xrange(len(self.joint_indices))], "ABS", "PENALIZE_LOW_HEIGHT_%i"%t) # do optimization result = trajoptpy.OptimizeProblem(prob) self.robot.SetDOFValues(start_joints, self.joint_indices) prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem num_upsampled_collisions = self._num_collisions(result.GetTraj()) print('Number of collisions: {0}'.format(num_upsampled_collisions)) self.robot.SetDOFValues(start_joints, self.joint_indices) if num_upsampled_collisions > 2: return None else: return result.GetTraj()
def main(): import yaml import os; import os.path as osp import sys sys.path.insert(1, os.path.join(sys.path[0], '..')); import planning_benchmark_common as pbc import argparse parser = argparse.ArgumentParser() parser.add_argument("problemfile", type=argparse.FileType("r")) parser.add_argument("--base_only", action="store_true") args = parser.parse_args() problemset = yaml.load(args.problemfile) assert problemset["active_affine"] == 11 env = openravepy.Environment() env.StopSimulation() env.Load(osp.join(pbc.envfile_dir,problemset["env_file"])) robot2file = { "pr2":"robots/pr2-beta-static.zae" } env.Load(robot2file[problemset["robot_name"]]) robot = env.GetRobots()[0] robot.SetTransform(openravepy.matrixFromPose(problemset["default_base_pose"])) rave_joint_names = [joint.GetName() for joint in robot.GetJoints()] rave_inds, rave_values = [],[] for (name,val) in zip(problemset["joint_names"], problemset["default_joint_values"]): if name in rave_joint_names: i = rave_joint_names.index(name) rave_inds.append(i) rave_values.append(val) robot.SetDOFValues(rave_values, rave_inds) active_joint_inds = [rave_joint_names.index(name) for name in problemset["active_joints"]] robot.SetActiveDOFs(active_joint_inds, problemset["active_affine"]) env.SetViewer('qtcoin') env.UpdatePublishedBodies() if args.base_only: poses = sample_base_positions(robot, num=100) else: poses = sample_all(robot, num=100) while True: for i, p in enumerate(poses): robot.SetActiveDOFValues(poses[i]) env.UpdatePublishedBodies() raw_input('showing %d/%d: joints = %s' % (i+1, len(poses), ', '.join(str(v) for v in poses[i]))) print '=================='
def get_lin_interp_poses(start_pos, end_pos, n_steps): xyz_start, quat_start = juc.hmat_to_trans_rot(start_pos) xyz_end, quat_end = juc.hmat_to_trans_rot(end_pos) xyzs = math_utils.linspace2d(xyz_start, xyz_end, n_steps) quats = [ rave.quatSlerp(quat_start, quat_end, t) for t in np.linspace(0, 1, n_steps) ] hmats = [ rave.matrixFromPose(np.r_[quat[3], quat[:3], xyz]) for (xyz, quat) in zip(xyzs, quats) ] gripper_poses = np.array([juc.hmat_to_pose(hmat) for hmat in hmats]) return gripper_poses
def _init_robot(self): """ Initialize the robot """ # set initial pose self.robot.SetTransform( rave.matrixFromPose(np.array([1, 0, 0, 0, 0, 0, 0]))) self.robot.SetDOFValues([0.54, -1.57, 1.57, 0.54], [22, 27, 15, 34]) # get robot manipulation tools self.manip_ = self.robot.SetActiveManipulator("leftarm_torso") self.maniprob_ = rave.interfaces.BaseManipulation( self.robot) # create the interface for task manipulation programs self.taskprob_ = rave.interfaces.TaskManipulation( self.robot) # create the interface for task manipulation programs self.finger_joint_ = self.robot.GetJoint('l_gripper_l_finger_joint')
def PublishWorkspaceTrajectorySegments(rviz_tools, robot, traj, color='red', line_width=0.005): """ Draw the linear segment between each waypoint of a workspace trajectory as a line using Rviz Markers. This works because the trajectory has linear interpolation for the translation component. @param instance rviz_tools: An instance of the rviz_tools.RvizMarkers class @param openravepy.robot robot: The robot. @param openravepy.Trajectory traj: A workspace trajectory. @param color string: Color name to draw the line segments. @param line_width float: Width of the line segments. """ num_waypoints = traj.GetNumWaypoints() T_prev = openravepy.matrixFromPose(traj.GetWaypoint(0)[0:7]) for i in range(1, num_waypoints): P = traj.GetWaypoint(i)[0:7] # first 7 values are pose T = openravepy.matrixFromPose(P) duration = None # None = publish forever rviz_tools.publishLine(T_prev, T, color, line_width, duration) T_prev = T
def get_robot_ee_position(self, msg): # ee_orientation = pyquaternion.Quaternion(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z) # ee_position = numpy.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]) pose = [ msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.position.x, msg.pose.position.y, msg.pose.position.z ] pose = openravepy.matrixFromPose(pose) ee_pose = numpy.dot(pose, self.T_G2EE) ee_pose = openravepy.poseFromMatrix(ee_pose) self.ee_orientation = pyquaternion.Quaternion(ee_pose[:4]) self.ee_position = numpy.array(ee_pose[4:])
def PublishWorkspaceTrajectoryClosestSegment(rviz_tools, robot, traj, color='red', line_width=0.005, t_min=None, t_max=None): """ Draw the linear segment of a workspace trajectory which is closest to the segment from t_min to t_max. """ if (t_min == None) or (t_max == None): raise ValueError("You must specify the desired segment using " "t_min and t_max in " "PublishWorkspaceTrajectoryClosestSegment()") waypoint_times = GetWorkspaceTrajectoryWaypointTimes(traj) epsilon = 0.000001 T_prev = openravepy.matrixFromPose(traj.GetWaypoint(0)[0:7]) waypoint_time_prev = 0.0 num_waypoints = traj.GetNumWaypoints() for i in range(1, num_waypoints): P = traj.GetWaypoint(i)[0:7] # first 7 values are pose T = openravepy.matrixFromPose(P) waypoint_time = waypoint_times[i] diff_from_t_min = abs(waypoint_time_prev - t_min) diff_from_t_max = abs(waypoint_time - t_max) # If this segment starts and ends outside the desired time # limits then draw it if (t_min+epsilon >= waypoint_time_prev) \ and (t_max-epsilon <= waypoint_time): duration = None # None = publish forever rviz_tools.publishLine(T_prev, T, color, line_width, duration) T_prev = T waypoint_time_prev = waypoint_time
def cylinder_collisionfree_model(env, body_name, pos, dims): iv_str = '#Inventor V2.1 ascii\nSeparator {\nCylinder {\nparts ALL\nradius %f\nheight %f\n}\n}'%(dims[0], dims[1]) # hardcode collision-free disk dimensions with open("../models/cylinder.iv", "w+") as f: f.write(iv_str) xml_str = '<KinBody name="%s"> <Body name="surface" type="dynamic"> <Geom type="sphere"> <Render>cylinder.iv</Render> <Radius>0.0001</Radius> \ <Translation>0 0 %f</Translation> <RotationAxis>1 0 0 90></RotationAxis> </Geom> </Body> </KinBody>'%(body_name, dims[1] / 2) with open("../models/temp_cylinder.xml", "w+") as f: f.write(xml_str) env.Load("../models/temp_cylinder.xml") cylinder = env.GetKinBody(body_name) x, y, z = pos cylinder_t = openravepy.matrixFromPose([1, 0, 0, 0, x, y, z]) cylinder.SetTransform(cylinder_t) return cylinder
def warehouse_scene_pairwise_test(initial_state_id, goal_constraint_starts_with, robot, arm="right", planner_id=""): from warehouse_utils import MoveitWarehouseDatabase constraints_db = MoveitWarehouseDatabase('moveit_constraints') states_db = MoveitWarehouseDatabase('moveit_robot_states') initial_state = states_db.get_message(RobotState, state_id=initial_state_id) query = { 'constraints_id': { "$regex": "^%s.*" % goal_constraint_starts_with } } constraints = constraints_db.get_messages(Constraints, query) print "Got", len(constraints), "goal constraints from the warehouse" initial_states = [initial_state] positions, quats = [], [] results = [] 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) update_rave_from_ros(robot, initial_state.joint_state.position, initial_state.joint_state.name) for c in constraints: pos, quat = pose_constraints_to_xyzquat(c) post = (pos.x, pos.y, pos.z) quatt = (quat.x, quat.y, quat.z, quat.w) positions.append(post) quats.append(quatt) state = robot_state_from_pose_goal(post, quatt, arm, robot, initial_state) if state: initial_states.append(state) for start_state in initial_states: update_rave_from_ros(robot, start_state.joint_state.position, start_state.joint_state.name) for posit, quater in zip(positions, quats): result = test_plan_to_pose(posit, quater, arm, robot, start_state, planner_id, "%s_wrist_roll_link") if result is not None: results.append(result) process_results(results)
def test_drive_base(pose_targ): wxyz_targ = pose_targ[:4] xyz_targ = pose_targ[4:] robot.SetDOFValues([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) robot.SetTransform(rave.matrixFromPose([1, 0, 0, 0, -3.4, -1.4, 0.05])) robot.SetActiveDOFs(np.r_[robot.GetManipulator("rightarm").GetArmIndices(), robot.GetLink("torso_lift_link").GetIndex()], rave.DOFAffine.X + rave.DOFAffine.Y + rave.DOFAffine.RotationAxis, [0,0,1]) angle_init = np.random.rand() * 2*np.pi x_init = xyz_targ[0] - .5*np.cos(angle_init) y_init = xyz_targ[1] - .5*np.sin(angle_init) dofvals_init = np.r_[np.zeros(7), 0, x_init, y_init, angle_init] #start = robot. start = robot.GetActiveDOFValues(); end = dofvals_init n_steps = 10; init_traj = mu.linspace2d(start, end, n_steps) manipname = "active" prob_desc = make_request_skeleton(manipname, n_steps); prob_desc["basic_info"]["start_fixed"] = True prob_desc["costs"].append( {"type" : "pose", "name" : "pose", "params" : { "pos_coeffs" : [10,10,10], "rot_coeffs" : [0,0,0], "xyz" : xyz_targ, "wxyz" : wxyz_targ, "link" : "r_gripper_tool_frame", }}) prob_desc["init_info"]["type"] = "given_traj" angle = np.linalg.norm(rave.axisAngleFromRotationMatrix(robot.GetTransform())) prob_desc["init_info"]["data"] = [row.tolist() for row in init_traj] s = json.dumps(prob_desc) traj.SetInteractive(True); prob = traj.ConstructProblem(s, env) return traj.OptimizeProblem(prob)
def init_env(problemset): env = openravepy.Environment() env.StopSimulation() robot2file = {"pr2": "robots/pr2-beta-static.zae"} if args.planner == "trajopt": if args.interactive: trajoptpy.SetInteractive(True) plan_func = trajopt_plan elif args.planner == "ompl": setup_ompl(env) plan_func = ompl_plan elif args.planner in ["chomp", "chomp2"]: setup_chomp(env) plan_func = chomp_plan # chomp needs a robot with spheres chomp_pr2_file = "pr2_with_spheres.robot.xml" if problemset[ "active_affine"] == 0 else "pr2_with_spheres_fullbody.robot.xml" robot2file["pr2"] = osp.join(pbc.envfile_dir, chomp_pr2_file) env.Load(osp.join(pbc.envfile_dir, problemset["env_file"])) env.Load(robot2file[problemset["robot_name"]]) robot = env.GetRobots()[0] if args.planner == "trajopt": try: postsetup_trajopt(env) except Exception: print "can't find ros config file. skipping self collision ignore" robot.SetTransform( openravepy.matrixFromPose(problemset["default_base_pose"])) rave_joint_names = [joint.GetName() for joint in robot.GetJoints()] rave_inds, rave_values = [], [] for (name, val) in zip(problemset["joint_names"], problemset["default_joint_values"]): if name in rave_joint_names: i = rave_joint_names.index(name) rave_inds.append(i) rave_values.append(val) robot.SetDOFValues(rave_values, rave_inds) active_joint_inds = [ rave_joint_names.index(name) for name in problemset["active_joints"] ] robot.SetActiveDOFs(active_joint_inds, problemset["active_affine"]) return env, robot, plan_func
def move_to_pregrasp(self, T_gripper_obj, eps=1e-2): """ Move the gripper to the pregrasp pose given by the grasp object """ # get grasp pose in rave T_mesh_obj = self.gripper_.T_mesh_gripper.dot(T_gripper_obj) gripper_position = T_mesh_obj.inverse().pose.position gripper_orientation = T_mesh_obj.inverse().pose.orientation gripper_pose = np.array([ gripper_orientation.w, gripper_orientation.x, gripper_orientation.y, gripper_orientation.z, gripper_position.x, gripper_position.y, gripper_position.z ]) T_mesh_obj = rave.matrixFromPose(gripper_pose) self.gripper_obj_.SetTransform(T_mesh_obj) return T_mesh_obj
def create_cylinder(env, table, radius, height, body_name, max_radial_distance, rand, color): robot_pos = openravepy.poseFromMatrix(env.GetRobots()[0].GetTransform())[4:7] min_x, max_x, min_y, max_y, table_height = utils.get_object_limits(table) x = rand.uniform(min_x + radius, max_x - radius) y = rand.uniform(min_y + radius, max_y - radius) # while (x - robot_pos[0])**2 + (y - robot_pos[1])**2 > max_radial_distance: # x = rand.uniform(min_x + radius, max_x - radius) # y = rand.uniform(min_y + radius, max_y - radius) z = table_height + height / 2 t = openravepy.matrixFromPose([1, 0, 0, 0, x, y, z]) cylinder = object_models.create_cylinder(env, body_name, t, [radius, height], color) env.Add(cylinder, False) return cylinder
def plot_body_com(link, handle=None, color=None): """ efficiently plot the center of mass of a given link""" if color is None: color = _np.array([0, 1, 0]) origin = link.GetGlobalCOM() mass = link.GetMass() #Fetch environment from robot parent env = link.GetParent().GetEnv() if handle is None: handle = env.plot3(points=origin, pointsize=5.0 * mass, colors=color) else: neworigin = [1, 0, 0, 0] neworigin.extend(origin.tolist()) handle.SetTransform(_rave.matrixFromPose(neworigin)) return handle
def plot_body_com(link, handle=None, color=None): """ efficiently plot the center of mass of a given link""" if color is None: color = _np.array([0, 1, 0]) origin = link.GetGlobalCOM() mass = link.GetMass() #Fetch environment from robot parent env = link.GetParent().GetEnv() if handle is None: handle = env.plot3( points=origin, pointsize=5.0 * mass, colors=color) else: neworigin = [1, 0, 0, 0] neworigin.extend(origin.tolist()) handle.SetTransform(_rave.matrixFromPose(neworigin)) return handle
def init_env(problemset): env = openravepy.Environment() env.StopSimulation() robot2file = { "pr2":"robots/pr2-beta-static.zae" } if args.planner == "trajopt": if args.interactive: trajoptpy.SetInteractive(True) plan_func = trajopt_plan elif args.planner == "ompl": setup_ompl(env) plan_func = ompl_plan elif args.planner == "chomp": setup_chomp(env) plan_func = chomp_plan # chomp needs a robot with spheres chomp_pr2_file = "pr2_with_spheres.robot.xml" if problemset["active_affine"] == 0 else "pr2_with_spheres_fullbody.robot.xml" robot2file["pr2"] = osp.join(pbc.envfile_dir, chomp_pr2_file) env.Load(osp.join(pbc.envfile_dir,problemset["env_file"])) env.Load(robot2file[problemset["robot_name"]]) robot = env.GetRobots()[0] if args.planner == "trajopt": try: postsetup_trajopt(env) except Exception: print "can't find ros config file. skipping self collision ignore" robot.SetTransform(openravepy.matrixFromPose(problemset["default_base_pose"])) rave_joint_names = [joint.GetName() for joint in robot.GetJoints()] rave_inds, rave_values = [],[] for (name,val) in zip(problemset["joint_names"], problemset["default_joint_values"]): if name in rave_joint_names: i = rave_joint_names.index(name) rave_inds.append(i) rave_values.append(val) robot.SetDOFValues(rave_values, rave_inds) active_joint_inds = [rave_joint_names.index(name) for name in problemset["active_joints"]] robot.SetActiveDOFs(active_joint_inds, problemset["active_affine"]) return env, robot, plan_func
def add_body(env, body_name, box_msg, scale=1.0, padding=0): ###@$!@#$!@#!!! openrave takes half extents values = [0,0,0,\ box_msg.box_dims.x*scale/2 + padding,\ box_msg.box_dims.y*scale/2 + padding,\ box_msg.box_dims.z*scale/2 + padding] # values = [box_msg.pose.pose.position.x, \ # box_msg.pose.pose.position.y,\ # box_msg.pose.pose.position.z,\ # box_msg.box_dims.x, \ # box_msg.box_dims.y,\ # box_msg.box_dims.z] print(values) print("box_msg: "+ repr(box_msg)) body = openravepy.RaveCreateKinBody(env,'') body.SetName(body_name) body.InitFromBoxes(np.array([values]), True) T = body.GetTransform() # T[:,-1] = [box_msg.pose.pose.position.x, \ # box_msg.pose.pose.position.y, \ # box_msg.pose.pose.position.z, 1] obj_position = [box_msg.pose.pose.position.x,\ box_msg.pose.pose.position.y,\ box_msg.pose.pose.position.z + \ (box_msg.box_dims.z*scale/2 + padding - box_msg.box_dims.z/2)] ###@$!@#$!@#!!! Openrave expresses quaternions as wxyz!!! obj_rot = [box_msg.pose.pose.orientation.w,\ box_msg.pose.pose.orientation.x,\ box_msg.pose.pose.orientation.y,\ box_msg.pose.pose.orientation.z] poseForOpenrave = [] poseForOpenrave[0:4] = obj_rot poseForOpenrave[4:6] = obj_position # T = openravepy.matrixFromQuat(obj_rot) # body.SetTransform(T) T = openravepy.matrixFromPose(poseForOpenrave) body.SetTransform(T) env.Add(body, True)
def spawn_table_pr2(env, robot_dist_from_table, tabletype='rll'): if tabletype == 'rll': thickness = 0.2 legheight = 0.6 table = object_models.create_table(env, TABLE_NAMES[tabletype], 2.235, 0.94, thickness, 1.3, 0.6, legheight) elif tabletype == 'small': thickness = 0.2 legheight = 0.6 table = object_models.create_table(env, TABLE_NAMES[tabletype], 0.7 - robot_dist_from_table, 1.5, thickness, 0.2 - robot_dist_from_table, 0.2, legheight) x = -utils.get_object_limits(table)[0] + 0.35 + robot_dist_from_table y = 0 z = thickness / 2 + legheight table.SetTransform(openravepy.matrixFromPose([1, 0, 0, 0, x, y, z])) # env.AddKinBody(table) env.AddRobot(table) # robot = env.ReadRobotXMLFile("../models/pr2/pr2-head-kinect.xml") robot = env.ReadRobotXMLFile("../models/pr2/pr2.zae") env.Add(robot)
def move_to_pregrasp(self, grasp_pose, eps=1e-2): """ Move the robot to the pregrasp pose given by the grasp object """ # get grasp pose gripper_position = grasp_pose.position gripper_orientation = grasp_pose.orientation gripper_pose = np.array([gripper_orientation.w, gripper_orientation.x, gripper_orientation.y, gripper_orientation.z, gripper_position.x, gripper_position.y, gripper_position.z]) if abs(np.linalg.norm(np.array(gripper_orientation.list)) - 1.0) > eps: logging.warning('Illegal pose') return None, None # get grasp pose relative to object T_gripper_obj = rave.matrixFromPose(gripper_pose) T_obj_world = self.T_gripper_world_.dot(self.T_rviz_or_).dot(np.linalg.inv(T_gripper_obj)) # set robot position as inverse of object (for viewing purposes) T_robot_world = np.linalg.inv(T_obj_world) self.robot.SetTransform(T_robot_world) return T_gripper_obj, T_robot_world
def build_joint_request(jvals, arm, robot, initial_state=INITIAL_ROBOT_STATE, planner_id=''): m = mm.MotionPlanRequest() m.group_name = "%s_arm" % arm m.start_state = initial_state m.planner_id = planner_id c = mm.Constraints() joints = robot.GetJoints() joint_inds = robot.GetManipulator("%sarm"%arm).GetArmIndices() c.joint_constraints = [mm.JointConstraint(joint_name=joints[joint_inds[i]].GetName(), position = jvals[i]) for i in xrange(len(jvals))] jc = mm.JointConstraint() m.goal_constraints = [c] m.allowed_planning_time = rospy.Duration(args.max_planning_time) 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) return m
def get_joint_trajectory(self, start_joints, target_pose, n_steps=20): """ Calls trajopt to plan collision-free trajectory :param start_joints: list of initial joints :param target_pose: desired pose of tool_frame (tfx.pose) :param n_steps: trajopt discretization :return list of joint values """ assert len(start_joints) == len(self.joint_indices) assert target_pose.frame.count('base_link') == 1 self.sim.update() # set start joint positions self.robot.SetDOFValues(start_joints, self.joint_indices) # initialize trajopt inputs rave_pose = tfx.pose(self.sim.transform_from_to(target_pose.matrix, target_pose.frame, 'world')) quat = rave_pose.orientation xyz = rave_pose.position quat_target = [quat.w, quat.x, quat.y, quat.z] xyz_target = [xyz.x, xyz.y, xyz.z] rave_mat = rave.matrixFromPose(np.r_[quat_target, xyz_target]) #init_joint_target = ku.ik_for_link(rave_mat, self.manip, self.tool_frame, filter_options=rave.IkFilterOptions.CheckEnvCollisions) #if init_joint_target is None: # rospy.loginfo('get_traj: IK failed') # return False init_joint_target = None request = self._get_trajopt_request(xyz_target, quat_target, init_joint_target, n_steps) # convert dictionary into json-formatted string s = json.dumps(request) # create object that stores optimization problem prob = trajoptpy.ConstructProblem(s, self.sim.env) # do optimization result = trajoptpy.OptimizeProblem(prob) return result.GetTraj()
def main(): ### Parameters ### ENV_FILE = "data/pr2test1.env.xml" N_STEPS = 15 XYZ_TARGET = [.5,0,.9] QUAT_TARGET = [1,0,0,0] INTERACTIVE = True LINK_NAME = "r_gripper_tool_frame" ################## ### Env setup #### env = rave.RaveGetEnvironment(1) if env is None: env = rave.Environment() env.StopSimulation() atexit.register(rave.RaveDestroy) env.Load(ENV_FILE) robot = env.GetRobots()[0] robot.SetDOFValues([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) robot.SetTransform(rave.matrixFromPose([1, 0, 0, 0, -3.4, -1.4, 0.05])) robot.SetActiveDOFs(np.r_[robot.GetManipulator("rightarm").GetArmIndices(), robot.GetJoint("torso_lift_joint").GetDOFIndex()], rave.DOFAffine.X + rave.DOFAffine.Y + rave.DOFAffine.RotationAxis, [0,0,1]) ################## for i_try in xrange(100): request = position_base_request(robot, LINK_NAME, XYZ_TARGET, QUAT_TARGET) s = json.dumps(request) print "REQUEST:",s trajoptpy.SetInteractive(INTERACTIVE); prob = trajoptpy.ConstructProblem(s, env) result = trajoptpy.OptimizeProblem(prob) if result.GetConstraints()[0][1] < 1e-3: break print "succeeded on try %i"%(i_try) print result
def GeneratePose(sensorPosition, targetPosition, l=(1, 0, 0)): '''Helps to determine a sensor pose just given a sensor position and view target. - Input sensorPosition: 3-element desired position of sensor placement. - Input targetPosition: 3-element position of object required to view. - Input l: Sensor LOS axis in base frame given identity orientation. - Returns T: 4x4 numpy array (transformation matrix) representing desired pose of end effector in the base frame. ''' v = targetPosition - sensorPosition vMag = norm(v) v = [v[0] / vMag, v[1] / vMag, v[2] / vMag] k = [ l[1] * v[2] - l[2] * v[1], l[2] * v[0] - l[0] * v[2], l[0] * v[1] - l[1] * v[0] ] theta = acos(l[0] * v[0] + l[1] * v[1] + l[2] * v[2]) q = tf.transformations.quaternion_about_axis(theta, k) return openravepy.matrixFromPose(numpy.r_[[q[3], q[0], q[1], q[2]], sensorPosition])
def get_return_from_grasp_joint_trajectory(self, start_joints, target_pose, n_steps=40): """ Calls trajopt to plan return from grasp collision-free trajectory :param start_joints: list of initial joints :param target_pose: desired pose of tool_frame (tfx.pose) :param n_steps: trajopt discretization :return None if traj not collision free, else list of joint values """ assert len(start_joints) == len(self.joint_indices) assert target_pose.frame.count('base_link') == 1 # set active manipulator and start joint positions self.robot.SetDOFValues(start_joints, self.joint_indices) # initialize trajopt inputs rave_pose = tfx.pose( self.sim.transform_from_to(target_pose.matrix, target_pose.frame, 'world')) quat = rave_pose.orientation xyz = rave_pose.position quat_target = [quat.w, quat.x, quat.y, quat.z] xyz_target = [xyz.x, xyz.y, xyz.z] rave_mat = rave.matrixFromPose(np.r_[quat_target, xyz_target]) request = self._get_return_from_grasp_trajopt_request( xyz_target, quat_target, n_steps) # convert dictionary into json-formatted string s = json.dumps(request) # create object that stores optimization problem prob = trajoptpy.ConstructProblem(s, self.sim.env) tool_link = self.robot.GetLink(self.tool_frame) def penalize_low_height(x): self.robot.SetDOFValues(x, self.joint_indices, False) z = tool_link.GetTransform()[2, 3] return max(0, 10.0 - z) for t in xrange(n_steps - 2): prob.AddErrorCost(penalize_low_height, [(t, j) for j in xrange(len(self.joint_indices))], "ABS", "PENALIZE_LOW_HEIGHT_%i" % t) # do optimization result = trajoptpy.OptimizeProblem(prob) self.robot.SetDOFValues(start_joints, self.joint_indices) prob.SetRobotActiveDOFs( ) # set robot DOFs to DOFs in optimization problem num_upsampled_collisions = self._num_collisions(result.GetTraj()) print('Number of collisions: {0}'.format(num_upsampled_collisions)) self.robot.SetDOFValues(start_joints, self.joint_indices) if num_upsampled_collisions > 2: return None else: return result.GetTraj()
# import xml.etree.ElementTree as ET # cc = trajoptpy.GetCollisionChecker(env) # root = ET.parse("/opt/ros/groovy/share/pr2_moveit_config/config/pr2.srdf").getroot() # disabled_elems=root.findall("disable_collisions") # for elem in disabled_elems: # linki = robot.GetLink(elem.get("link1")) # linkj = robot.GetLink(elem.get("link2")) # if linki and linkj: # cc.ExcludeCollisionPair(linki, linkj) xs, ys, zs = np.mgrid[.35:.85:.05, 0:.5:.05, .8:.9:.1] wxyz = [1, 0, 0, 0] results = [] for (x, y, z) in zip(xs.flat, ys.flat, zs.flat): wxyzxyz = np.r_[wxyz, x, y, z] joint_solutions = ku.ik_for_link(openravepy.matrixFromPose(wxyzxyz), robot.GetManipulator(manip), "%s_gripper_tool_frame" % manip[0], 1, True) if len(joint_solutions) > 0: target_joints = joint_solutions[0] results.append( plan(robot, manip, target_joints, wxyzxyz if args.pose_goal else None)) success_frac = np.mean([result.success for result in results]) print "success frac:", success_frac print "avg total time:", np.mean([result.t_total for result in results]) print "avg optimization time:", np.mean([result.t_opt for result in results]) print "avg verification time:", np.mean( [result.t_verify for result in results])
def PlanWorkspacePath(self, robot, traj, timelimit=5.0, min_waypoint_index=None, norm_order=2, **kw_args): """ Plan a configuration space path given a workspace path. All timing information is ignored. @param robot @param traj workspace trajectory represented as OpenRAVE AffineTrajectory @param min_waypoint_index minimum waypoint index to reach @param timelimit timeout in seconds @param norm_order: 1 ==> The L1 norm 2 ==> The L2 norm inf ==> The L_infinity norm Used to determine the resolution of collision checked waypoints in the trajectory @return qtraj configuration space path """ env = robot.GetEnv() from .exceptions import (TimeoutPlanningError, CollisionPlanningError, SelfCollisionPlanningError) from openravepy import (CollisionOptions, CollisionOptionsStateSaver, CollisionReport) p = openravepy.KinBody.SaveParameters with robot, CollisionOptionsStateSaver(env.GetCollisionChecker(), CollisionOptions.ActiveDOFs), \ robot.CreateRobotStateSaver(p.ActiveDOF | p.LinkTransformation): manip = robot.GetActiveManipulator() robot.SetActiveDOFs(manip.GetArmIndices()) # Create a new trajectory starting at current robot location. qtraj = openravepy.RaveCreateTrajectory(env, '') qtraj.Init(manip.GetArmConfigurationSpecification('linear')) qtraj.Insert(0, robot.GetActiveDOFValues()) # Initial search for workspace path timing: one huge step. t = 0. dt = traj.GetDuration() q_resolutions = robot.GetActiveDOFResolutions() # Smallest CSpace step at which to give up min_step = numpy.linalg.norm(robot.GetActiveDOFResolutions() / 100., ord=norm_order) ik_options = openravepy.IkFilterOptions.CheckEnvCollisions start_time = time.time() epsilon = 1e-6 try: while t < traj.GetDuration() + epsilon: # Check for a timeout. # TODO: This is not really deterministic because we do not # have control over CPU time. However, it is exceedingly # unlikely that running the query again will change the # outcome unless there is a significant change in CPU load. current_time = time.time() if (timelimit is not None and current_time - start_time > timelimit): raise TimeoutPlanningError(timelimit, deterministic=True) # Hypothesize new configuration as closest IK to current qcurr = robot.GetActiveDOFValues() # Configuration at t. qnew = manip.FindIKSolution(openravepy.matrixFromPose( traj.Sample(t + dt)[0:7]), ik_options, ikreturn=False, releasegil=True) # Check if the step was within joint DOF resolution. infeasible_step = True if qnew is not None: # Found an IK steps = abs(qnew - qcurr) / q_resolutions norm = numpy.linalg.norm(steps, ord=norm_order) if (norm < min_step) and qtraj: raise PlanningError('Not making progress.') infeasible_step = norm > 1.0 if infeasible_step: # Backtrack and try half the step dt = dt / 2.0 else: # Move forward to new trajectory time. robot.SetActiveDOFValues(qnew) qtraj.Insert(qtraj.GetNumWaypoints(), qnew) t = min(t + dt, traj.GetDuration()) dt = dt * 2.0 except PlanningError as e: # Compute the min acceptable time from the min waypoint index. if min_waypoint_index is None: min_waypoint_index = traj.GetNumWaypoints() - 1 cspec = traj.GetConfigurationSpecification() wpts = [ traj.GetWaypoint(i) for i in range(min_waypoint_index + 1) ] dts = [cspec.ExtractDeltaTime(wpt) for wpt in wpts] min_time = numpy.sum(dts) # Throw an error if we haven't reached the minimum waypoint. if t < min_time: # FindIKSolutions is slower than FindIKSolution, so call # this only to identify error when there is no solution. ik_solutions = manip.FindIKSolutions( openravepy.matrixFromPose( traj.Sample(t + dt * 2.0)[0:7]), openravepy.IkFilterOptions.IgnoreSelfCollisions, ikreturn=False, releasegil=True) collision_error = None # update collision_error to contain collision info. with robot.CreateRobotStateSaver(p.LinkTransformation): for q in ik_solutions: robot.SetActiveDOFValues(q) cr = CollisionReport() if env.CheckCollision(robot, report=cr): collision_error = \ CollisionPlanningError.FromReport( cr, deterministic=True) elif robot.CheckSelfCollision(report=cr): collision_error = \ SelfCollisionPlanningError.FromReport( cr, deterministic=True) else: collision_error = None if collision_error is not None: raise collision_error else: raise # Otherwise we'll gracefully terminate. else: logger.warning('Terminated early at time %f < %f: %s', t, traj.GetDuration(), str(e)) SetTrajectoryTags(qtraj, { Tags.CONSTRAINED: True, Tags.DETERMINISTIC_TRAJECTORY: True, Tags.DETERMINISTIC_ENDPOINT: True, }, append=True) return qtraj
trajoptpy.SetInteractive( args.interactive ) # pause every iteration, until you press 'p'. Press escape to disable further plotting robot = env.GetRobots()[0] manip = robot.GetManipulator("rightarm") robot.SetActiveManipulator(manip) ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot, iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() xyz_target = [.6, -.6, 1] n_steps = 15 hmat_target = openravepy.matrixFromPose(np.r_[(0, 1, 0, 0), xyz_target]) init_joint_target = ku.ik_for_link( hmat_target, manip, "r_gripper_tool_frame", filter_options=openravepy.IkFilterOptions.CheckEnvCollisions) request = { "basic_info": { "n_steps": n_steps, "manip": "rightarm", "start_fixed": True }, "costs": [{ "type": "joint_vel",
env.StopSimulation() env.Load("robots/pr2-beta-static.zae") env.Load("../data/table.xml") trajoptpy.SetInteractive( args.interactive ) # pause every iteration, until you press 'p'. Press escape to disable further plotting robot = env.GetRobots()[0] joint_start = [-1.832, -0.332, -1.011, -1.437, -1.1, -1.926, 3.074] robot.SetDOFValues(joint_start, robot.GetManipulator('rightarm').GetArmIndices()) quat_target = [1, 0, 0, 0] # wxyz xyz_target = [6.51073449e-01, -1.87673551e-01, 4.91061915e-01] hmat_target = openravepy.matrixFromPose(np.r_[quat_target, xyz_target]) # BEGIN ik manip = robot.GetManipulator("rightarm") robot.SetActiveManipulator(manip) ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot, iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() init_joint_target = ku.ik_for_link( hmat_target, manip, "r_gripper_tool_frame", filter_options=openravepy.IkFilterOptions.CheckEnvCollisions) # END ik
def get_grasp_joint_trajectory(self, start_joints, target_pose, n_steps=40, ignore_orientation=False, link_name=None): """ Calls trajopt to plan grasp collision-free trajectory :param start_joints: list of initial joints :param target_pose: desired pose of tool_frame (tfx.pose) :param n_steps: trajopt discretization :return None if traj not collision free, else list of joint values """ link_name = link_name if link_name is not None else self.tool_frame assert len(start_joints) == len(self.joint_indices) assert target_pose.frame.count('base_link') == 1 self.sim.update() # set active manipulator and start joint positions self.robot.SetDOFValues(start_joints, self.joint_indices) # initialize trajopt inputs rave_pose = tfx.pose( self.sim.transform_from_to(target_pose.matrix, target_pose.frame, 'world')) quat = rave_pose.orientation xyz = rave_pose.position quat_target = [quat.w, quat.x, quat.y, quat.z] xyz_target = [xyz.x, xyz.y, xyz.z] rave_mat = rave.matrixFromPose(np.r_[quat_target, xyz_target]) # init_joint_target = None init_joint_target = self.sim.ik_for_link(rave_pose.matrix, self.manip, link_name, 0) if init_joint_target is not None: init_joint_target = self._closer_joint_angles( init_joint_target, start_joints) init_traj = self.ik_point(start_joints, xyz, n_steps=n_steps, link_name=link_name) request = self._get_grasp_trajopt_request( xyz_target, quat_target, n_steps, ignore_orientation=ignore_orientation, link_name=link_name, init_traj=init_traj) # convert dictionary into json-formatted string s = json.dumps(request) # create object that stores optimization problem prob = trajoptpy.ConstructProblem(s, self.sim.env) # TODO: worth doing? # tool_link = self.robot.GetLink(link_name) # def point_at(x): # self.robot.SetDOFValues(x, self.joint_indices, False) # T = tool_link.GetTransform() # local_dir = xyz.array - T[:3,3] # return T[1:3,:3].dot(local_dir) # # for t in xrange(int(0.8*n_steps), n_steps-1): # #prob.AddConstraint(point_at, [(t,j) for j in xrange(len(self.joint_indices))], "EQ", "POINT_AT_%i"%t) # prob.AddErrorCost(point_at, [(t,j) for j in xrange(len(self.joint_indices))], "ABS", "POINT_AT_%i"%t) # do optimization result = trajoptpy.OptimizeProblem(prob) prob.SetRobotActiveDOFs( ) # set robot DOFs to DOFs in optimization problem #num_upsampled_collisions = len(traj_collisions(result.GetTraj(), self.robot, n=100)) num_upsampled_collisions = self._num_collisions(result.GetTraj()) print('Number of collisions: {0}'.format(num_upsampled_collisions)) self.robot.SetDOFValues(start_joints, self.joint_indices) if num_upsampled_collisions > 2: #if not traj_is_safe(result.GetTraj()[:], self.robot): # Check that trajectory is collision free return None else: return result.GetTraj()