def VerifyIKSolver(youbot, desired_eepose, visualize=False): if visualize: h = orpy.misc.DrawAxes(youbot.GetEnv(), desired_eepose, 0.25, 2) time.sleep(1) base_poses,arm_configs = FindIKAndBaseSolutions(youbot,desired_eepose,returnfirst=False,checkenvcollision=False,randomize=False) print 'Found %d IK solutions. Verifying...'%(len(base_poses)) youbot.GetController().Reset() for pose,q,i in izip(base_poses,arm_configs,range(len(base_poses))): with youbot: youbot.SetTransform(pose) youbot.SetDOFValues(q,youbot.GetManipulators()[0].GetArmIndices()) eepose = youbot.GetManipulators()[0].GetEndEffectorTransform() if np.linalg.norm(eepose[:3,3]-desired_eepose[:3,3]) > 0.005: print 'EE is not at the desired position. index: %d.'%(i) return base_poses,arm_configs quat1 = orpy.quatFromRotationMatrix(desired_eepose[:3,:3]) quat2 = orpy.quatFromRotationMatrix(eepose[:3,:3]) quatinnerproduct = quat1[0]*quat2[0]+quat1[1]*quat2[1]+quat1[2]*quat2[2]+quat1[3]*quat2[3] quatdist = np.arccos(2.0* (quatinnerproduct*quatinnerproduct) - 1.0) if quatdist > 0.001: print 'EE is not at the desired orientation. index: %d.'%(i) return base_poses,arm_configs if visualize: youbot.GetEnv().UpdatePublishedBodies() time.sleep(0.001) print 'All IK solutions verified.' h = None return base_poses,arm_configs
def VerifyIKSolver(youbot, desired_eepose, visualize=False): if visualize: h = orpy.misc.DrawAxes(youbot.GetEnv(), desired_eepose, 0.25, 2) time.sleep(1) base_poses, arm_configs = FindIKAndBaseSolutions(youbot, desired_eepose, returnfirst=False, checkenvcollision=False, randomize=False) print 'Found %d IK solutions. Verifying...' % (len(base_poses)) youbot.GetController().Reset() for pose, q, i in izip(base_poses, arm_configs, range(len(base_poses))): with youbot: youbot.SetTransform(pose) youbot.SetDOFValues(q, youbot.GetManipulators()[0].GetArmIndices()) eepose = youbot.GetManipulators()[0].GetEndEffectorTransform() if np.linalg.norm(eepose[:3, 3] - desired_eepose[:3, 3]) > 0.005: print 'EE is not at the desired position. index: %d.' % (i) return base_poses, arm_configs quat1 = orpy.quatFromRotationMatrix(desired_eepose[:3, :3]) quat2 = orpy.quatFromRotationMatrix(eepose[:3, :3]) quatinnerproduct = quat1[0] * quat2[0] + quat1[1] * quat2[ 1] + quat1[2] * quat2[2] + quat1[3] * quat2[3] quatdist = np.arccos(2.0 * (quatinnerproduct * quatinnerproduct) - 1.0) if quatdist > 0.001: print 'EE is not at the desired orientation. index: %d.' % (i) return base_poses, arm_configs if visualize: youbot.GetEnv().UpdatePublishedBodies() time.sleep(0.001) print 'All IK solutions verified.' h = None return base_poses, arm_configs
def GetStraightVelocity(self, manip, velocity, initial_hand_pose, nullspace_fn, step_size, sign_flipper = 1): robot = manip.GetRobot() current_hand_pose = manip.GetEndEffectorTransform() initial_position = initial_hand_pose[0:3, 3] current_position = current_hand_pose[0:3, 3] # Simulate a position goal step_size distance further along the velocity vector. moved_already = velocity * numpy.dot(current_position - initial_position, velocity) desired_position = initial_position + moved_already + velocity * step_size error_pos = desired_position - current_position # Append the desired quaternion to create the error vector. There is a # sign ambiguity on quaternions, so we'll always choose the shortest path. initial_ori = openravepy.quatFromRotationMatrix(initial_hand_pose) current_ori = openravepy.quatFromRotationMatrix(current_hand_pose) choices_ori = [ current_ori - initial_ori, current_ori + initial_ori ] error_ori = sign_flipper*min(choices_ori, key=lambda q: numpy.linalg.norm(q)) # Jacobian pseudo-inverse. jacobian_spatial = manip.CalculateJacobian() jacobian_angular = manip.CalculateRotationJacobian() #this function seems very buggy/wrong jacobian = numpy.vstack((jacobian_spatial, jacobian_angular)) jacobian_pinv = numpy.linalg.pinv(jacobian) # Null-space projector nullspace_projector = numpy.eye(jacobian.shape[1]) - numpy.dot(jacobian_pinv, jacobian) nullspace_goal = nullspace_fn(robot) pose_error = numpy.hstack((error_pos, error_ori)) return numpy.dot(jacobian_pinv, pose_error) + numpy.dot(nullspace_projector, nullspace_goal)
def straight_line_jacobian(self, distance): graph = np.array([]) points =np.array([self.robot.GetActiveDOFValues()]) delta_d = 0.001 distance_to_goal = 1 #constructing the jacobian j_spatial = self.manip.CalculateJacobian() j_rot = self.manip.CalculateRotationJacobian() J = np.vstack((j_spatial, j_rot)) Jinv = np.linalg.pinv(J) #pseudo-inverse #end effector data ee_trans = self.end_effector.GetTransform() ee_pos_init = ee_trans[0:3,3] #position of the end effector ee_quat_init = openravepy.quatFromRotationMatrix(ee_trans[0:3,0:3]) x_present = np.append(ee_pos_init, ee_quat_init) x_add = [delta_d, 0, 0, 0, 0, 0, 0] x_goal = x_present + x_add x_present_x = ee_pos_init[0] while np.abs(distance_to_goal) > delta_d: q_present = self.robot.GetActiveDOFValues() delta_x = x_goal - x_present delta_q = np.array(np.dot(Jinv,delta_x)) q_new = np.array(q_present + delta_q) check = self.check_collision(q_new) if(check): print ' the final positon of the end effector is:' print x_present[0:3] break else: with self.env: self.robot.SetActiveDOFValues(q_new) points = np.append(points, np.array([q_present + delta_q]), axis=0) j_spatial = self.manip.CalculateJacobian() j_rot = self.manip.CalculateRotationJacobian() J = np.vstack((j_spatial, j_rot)) Jinv = np.linalg.pinv(J) ee_trans = self.end_effector.GetTransform() ee_quat = openravepy.quatFromRotationMatrix(ee_trans[0:3,0:3]) ee_pos = ee_trans[0:3,3] x_present = np.append(ee_pos, ee_quat) x_present_x = x_present_x + delta_d x_goal = np.append([x_present_x, x_present[1], x_present[2]], ee_quat) distance_to_goal = x_goal[0]-(ee_pos_init[0]+distance) traj = self.points_to_traj(np.array(points)) #print points return traj
def rotation_interpolation(r1, r2, t): quat1 = rave.quatFromRotationMatrix(r1) quat2 = rave.quatFromRotationMatrix(r2) # print(r1) # print(r2) t = min(max(t, 0), 1) quat_interpolate = rave.quatSlerp(quat1, quat2, t, True) return rave.matrixFromQuat(quat_interpolate)[0:3, 0:3]
def IsSameGrasp(self,g1,g2): diff = g1[:3,3] - g2[:3,3] #if np.linalg.norm(diff) < 0.001: if (diff[0] ** 2 + diff[1] ** 2 + diff[2] ** 2) > 0.00001: return False qd=orpy.quatMult(orpy.quatInverse(orpy.quatFromRotationMatrix(g1[:3,:3])),orpy.quatFromRotationMatrix(g2[:3,:3])) shortest_arc_angle = 2.0*np.arctan2(np.linalg.norm(qd[1:4]),qd[0]) if shortest_arc_angle > 0.001: return False return True
def IsSameTransform(tr1, tr2): # FIXME just check element-by-element similarity? if np.linalg.norm(tr1[:3,3]-tr2[:3,3]) > 0.0001: return False quat1 = orpy.quatFromRotationMatrix(tr1[:3,:3]) quat2 = orpy.quatFromRotationMatrix(tr2[:3,:3]) quatinnerproduct = quat1[0]*quat2[0]+quat1[1]*quat2[1]+quat1[2]*quat2[2]+quat1[3]*quat2[3] quatdist = np.arccos(2.0* (quatinnerproduct*quatinnerproduct) - 1.0) if quatdist > 0.00001: return False return True
def IsSameTransform(tr1, tr2): # FIXME just check element-by-element similarity? if np.linalg.norm(tr1[:3, 3] - tr2[:3, 3]) > 0.0001: return False quat1 = orpy.quatFromRotationMatrix(tr1[:3, :3]) quat2 = orpy.quatFromRotationMatrix(tr2[:3, :3]) quatinnerproduct = quat1[0] * quat2[0] + quat1[1] * quat2[1] + quat1[ 2] * quat2[2] + quat1[3] * quat2[3] quatdist = np.arccos(2.0 * (quatinnerproduct * quatinnerproduct) - 1.0) if quatdist > 0.00001: return False return True
def IsSameGrasp(self, g1, g2): diff = g1[:3, 3] - g2[:3, 3] #if np.linalg.norm(diff) < 0.001: if (diff[0]**2 + diff[1]**2 + diff[2]**2) > 0.00001: return False qd = orpy.quatMult( orpy.quatInverse(orpy.quatFromRotationMatrix(g1[:3, :3])), orpy.quatFromRotationMatrix(g2[:3, :3])) shortest_arc_angle = 2.0 * np.arctan2(np.linalg.norm(qd[1:4]), qd[0]) if shortest_arc_angle > 0.001: return False return True
def RegisterBody(self, or_body, tf_id, openrave_frame_in_tf_frame=None, planar_tracking=False, fixed_translation_z=-1.0): x = 0.0 y = 0.0 z = 0.0 qw = 1.0 qx = 0.0 qy = 0.0 qz = 0.0 if openrave_frame_in_tf_frame is not None: x = openrave_frame_in_tf_frame[0,3] y = openrave_frame_in_tf_frame[1,3] z = openrave_frame_in_tf_frame[2,3] quat = orpy.quatFromRotationMatrix(openrave_frame_in_tf_frame[:3,:3]) qw = quat[0] qx = quat[1] qy = quat[2] qz = quat[3] cmd = 'RegisterBody ' + or_body.GetName() + ' ' + tf_id + ' openrave_frame_in_tf_frame ' + str(x) + ' ' + str(y) + ' ' + str(z) + ' ' + str(qw) + ' ' + str(qx) + ' ' + str(qy) + ' ' + str(qz) if planar_tracking: cmd +=' planar_tracking' + ' ' + 'fixed_translation_z' + ' ' + str(fixed_translation_z) self.or_tf.SendCommand(cmd)
def serialize_transform(t): from openravepy import quatFromRotationMatrix return { 'position': list(map(float,t[0:3, 3])), 'orientation': list(map(float,quatFromRotationMatrix(t[0:3, 0:3]))), }
def inverse_kinematics(robot, manip_name, ee_link_name, ee_hmat): init_guess = np.zeros(7) xyz_target = ee_hmat[:3, 3] quat_target = openravepy.quatFromRotationMatrix(ee_hmat[:3, :3]) request = { "basic_info": { "n_steps": 10, "manip": manip_name, # see below for valid values "start_fixed": False, # 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 }, { "type": "collision", "name": "cont_coll", # shorten name so printed table will be prettier "params": { "continuous": False, "coeffs": [ 50 ], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps "dist_pen": [0.05], # robot-obstacle distance that penalty kicks in. expands to length n_timesteps }, }, ], "constraints": [ # BEGIN pose_constraint { "type": "pose", "params": { "xyz": xyz_target.tolist(), "wxyz": quat_target.tolist(), "link": ee_link_name, "pos_coeffs": [10, 10, 10], "rot_coeffs": [10, 10, 10], }, } # END pose_constraint ], # BEGIN init "init_info": { "type": "straight_line", # straight line in joint space. "endpoint": init_guess.tolist(), # need to convert numpy array to list } # END init } s = json.dumps(request) prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem result = trajoptpy.OptimizeProblem(prob) # do optimization traj = result.GetTraj() return traj[-1]
def getPoseMsg(transform): #Convert transform into ROS poseStamped message robotPose = Pose() q = openravepy.quatFromRotationMatrix(transform[0:3, 0:3]) robotPose.position.x = transform[0][3] robotPose.position.y = transform[1][3] robotPose.position.z = transform[2][3] robotPose.orientation.x = q[1] robotPose.orientation.y = q[2] robotPose.orientation.z = q[3] robotPose.orientation.w = q[0] return robotPose
def straight_line_jacobian(self, distance): j_spatial = self.manip.CalculateJacobian() j_rot = self.manip.CalculateRotationJacobian() J = np.vstack((j_spatial, j_rot)) Jinv = np.linalg.pinv(J) #pseudo-inverse u,s,v=np.linalg.svd(J) quat = openravepy.quatFromRotationMatrix(trans) vh=v.T jac_null=vh[:,-1:] return jac_null
def GetStraightVelocity(self, manip, velocity, initial_hand_pose, nullspace_fn, step_size, sign_flipper=1): robot = manip.GetRobot() current_hand_pose = manip.GetEndEffectorTransform() initial_position = initial_hand_pose[0:3, 3] current_position = current_hand_pose[0:3, 3] # Simulate a position goal step_size distance further along the velocity vector. moved_already = velocity * numpy.dot( current_position - initial_position, velocity) desired_position = initial_position + moved_already + velocity * step_size error_pos = desired_position - current_position # Append the desired quaternion to create the error vector. There is a # sign ambiguity on quaternions, so we'll always choose the shortest path. initial_ori = openravepy.quatFromRotationMatrix(initial_hand_pose) current_ori = openravepy.quatFromRotationMatrix(current_hand_pose) choices_ori = [current_ori - initial_ori, current_ori + initial_ori] error_ori = sign_flipper * min(choices_ori, key=lambda q: numpy.linalg.norm(q)) # Jacobian pseudo-inverse. jacobian_spatial = manip.CalculateJacobian() jacobian_angular = manip.CalculateRotationJacobian( ) #this function seems very buggy/wrong jacobian = numpy.vstack((jacobian_spatial, jacobian_angular)) jacobian_pinv = numpy.linalg.pinv(jacobian) # Null-space projector nullspace_projector = numpy.eye(jacobian.shape[1]) - numpy.dot( jacobian_pinv, jacobian) nullspace_goal = nullspace_fn(robot) pose_error = numpy.hstack((error_pos, error_ori)) return numpy.dot(jacobian_pinv, pose_error) + numpy.dot( nullspace_projector, nullspace_goal)
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 AddGaussianNoise(self, orientationSigma, positionSigma): '''Returns a new grasp with noise added to the current position and orientation.''' descriptor = self if rand() < 0.5 else self.Flip() q = openravepy.quatFromRotationMatrix(descriptor.T) q += orientationSigma * randn(4) q /= norm(q) T = openravepy.matrixFromQuat(q) c = descriptor.center + positionSigma * descriptor.center T[0:3, 3] = c return HandDescriptor(\ T, image=None, function=descriptor.function, objectName=descriptor.objectName)
def calculate_sim_depths(xyz, rod, f): T_h_k = calc_Thk(xyz, rod) T_w_k = robot.GetLink("head_plate_frame").GetTransform().dot(T_h_k) sensor.SetPose(T_w_k[:3,3], openravepy.quatFromRotationMatrix(T_w_k[:3,:3])) sensor.SetIntrinsics(f) out = [] for dofs in rave_values: robot.SetActiveDOFValues(dofs) viewer.UpdateSceneData() sensor.Update() out.append(sensor.GetDepthImage()) return out
def update(self): import rospy with self.body.GetEnv(): or_pose = self.body.GetTransform() or_quaternion = openravepy.quatFromRotationMatrix(or_pose) position = tuple(or_pose[0:3, 3]) orientation = (or_quaternion[1], or_quaternion[2], or_quaternion[3], or_quaternion[0]) self.broadcaster.sendTransform(position, orientation, rospy.Time.now(), self.child_frame, self.parent_frame) self.timer = threading.Timer(self.period, self.update) self.timer.daemon = True self.timer.start()
def calculate_sim_depths(xyz, rod, f): T_h_k = calc_Thk(xyz, rod) T_w_k = robot.GetLink("head_plate_frame").GetTransform().dot(T_h_k) sensor.SetPose(T_w_k[:3, 3], openravepy.quatFromRotationMatrix(T_w_k[:3, :3])) sensor.SetIntrinsics(f) out = [] for dofs in rave_values: robot.SetActiveDOFValues(dofs) viewer.UpdateSceneData() sensor.Update() out.append(sensor.GetDepthImage()) return out
def RegisterBody(self, or_body, tf_id, openrave_frame_in_tf_frame=None, planar_tracking=False): if openrave_frame_in_tf_frame is None: self.tfplugin.SendCommand('RegisterBody ' + or_body.GetName() + ' ' + tf_id) else: x = openrave_frame_in_tf_frame[0, 3] y = openrave_frame_in_tf_frame[1, 3] z = openrave_frame_in_tf_frame[2, 3] quat = orpy.quatFromRotationMatrix( openrave_frame_in_tf_frame[:3, :3]) cmd = 'RegisterBody ' + or_body.GetName( ) + ' ' + tf_id + ' openrave_frame_in_tf_frame ' + str( x) + ' ' + str(y) + ' ' + str(z) + ' ' + str( quat[0]) + ' ' + str(quat[1]) + ' ' + str( quat[2]) + ' ' + str(quat[3]) if planar_tracking: cmd = cmd + ' planar_tracking' self.tfplugin.SendCommand(cmd)
def quat_from_rotation_matrix(R): return quatFromRotationMatrix(R)
def RosTransformFromRaveMatrix(mat): out = gm.Transform() out.translation = gm.Vector3(*mat[:3, 3]) qw, qx, qy, qz = openravepy.quatFromRotationMatrix(mat[:3, :3]) out.rotation = gm.Quaternion(qx, qy, qz, qw) return out
def PlanToEndEffectorOffset(self, robot, direction, distance, max_distance=None, position_tolerance=0.01, angular_tolerance=0.15, **kwargs): """ Plan to an end-effector offset with a move-hand-straight constraint. This function plans a trajectory to purely translate a hand in the given direction as far as possible before collision. Movement less than min_distance will return failure. The motion will not also move further than max_distance. @param robot the robot whose active DOFs will be used @param direction unit vector in the direction of motion @param distance minimum distance in meters @param max_distance maximum distance in meters @param position_tolerance constraint tolerance in meters @param angular_tolerance constraint tolerance in radians @return traj a trajectory following specified direction """ # Plan using the active manipulator. with robot.GetEnv(): manipulator = robot.GetActiveManipulator() pose = manipulator.GetEndEffectorTransform() # Convert IK endpoint transformation to pose. ee_position = pose[0:3, 3].tolist() ee_rotation = openravepy.quatFromRotationMatrix(pose).tolist() # Create dummy frame that has direction vector along Z. z = direction y = numpy.cross(z, pose[0:3, 0]) # Cross with EE-frame Y. y /= numpy.linalg.norm(y) x = numpy.cross(y, z) cost_frame = numpy.vstack((x, y, z)).T cost_rotation = openravepy.quatFromRotationMatrix(cost_frame).tolist() # Construct a planning request with these constraints. n_steps = 10 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] }, }, { "type": "pose", "params": { "xyz": [0, 0, 0], "wxyz": cost_rotation, "link": manipulator.GetEndEffector().GetName(), "rot_coeffs": [0, 0, 0], "pos_coeffs": [0, 0, 10] } }], "constraints": [{ "type": "pose", "params": { "xyz": ee_position, "wxyz": ee_rotation, "link": manipulator.GetEndEffector().GetName(), # "first_step": 0, # "last_step": n_steps-1, } }], "init_info": { "type": "stationary" } } # Set active DOFs to match active manipulator and plan. p = openravepy.KinBody.SaveParameters with robot.CreateRobotStateSaver(p.ActiveDOF): robot.SetActiveDOFs(manipulator.GetArmIndices()) return self._Plan(robot, request, **kwargs)
LINK_NAME = "l_gripper_tool_frame" #0.0640945 0.704273 -0.147528 0.691468 -0.168806 -0.0452678 0.762821 ################## ### Env setup #### env = rave.Environment() env.StopSimulation() 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.119839, 0.861339, -2.22045e-16, -1.87834, -3.00095, -1.02143, 3.0678, 0.54, -1.01863e-14, 0, -1.59595e-16, 0, 0, 0, 0, 0, 0, 0, 0, 0, -8.16014e-15, 0, -1.11022e-16, 0 ]) manip = robot.GetManipulator(MANIP_NAME) ################## T_gripper = robot.GetLink(LINK_NAME).GetTransform() T_grasp = T_gripper.copy() T_grasp[:3, 3] += np.array([0, 0.1, 0]) xyz_targ = T_grasp[:3, 3] success = mk.create_cylinder(env, xyz_targ - np.array([.03, 0, 0]), .02, .5) quat_targ = rave.quatFromRotationMatrix(T_grasp[:3, :3]) request = move_arm_to_grasp(xyz_targ, quat_targ, LINK_NAME, MANIP_NAME) s = json.dumps(request) print "REQUEST:", s trajoptpy.SetInteractive(args.interactive) prob = trajoptpy.ConstructProblem(s, env) result = trajoptpy.OptimizeProblem(prob)
def RosTransformFromRaveMatrix(mat): out = gm.Transform() out.translation = gm.Vector3(*mat[:3,3]) qw,qx,qy,qz = openravepy.quatFromRotationMatrix(mat[:3,:3]) out.rotation = gm.Quaternion(qx,qy,qz, qw) return out
def RosPoseFromRaveMatrix(mat): out = gm.Pose() out.position = gm.Point(*mat[:3,3]) qw,qx,qy,qz = openravepy.quatFromRotationMatrix(mat[:3,:3]) out.orientation = gm.Quaternion(qx,qy,qz, qw) return out
#robot.WaitForController(0) # wait #raw_input('Hit ENTER to continue.') ################## #T_gripper = robot.GetLink(LINK_NAME).GetTransform() #T_grasp = T_gripper.copy() #T_grasp[:3,3] += np.array([-0.5,0.2,0]) #T_grasp = T_grasp.dot(rave.matrixFromAxisAngle([0,0,1],np.pi/2)) #xyz_targ = T_grasp[:3,3] #success = mk.create_cylinder(env, xyz_targ-np.array([.03,0,0]), .02, .5) #quat_targ = rave.quatFromRotationMatrix(T_grasp[:3,:3]) #success = mk.create_cylinder(env, T_gripper[:3,3]-np.array([.1,-.1,0]), .02, .5) Tgoal = np.array([[0,0,1,0.6],[0,1,0,0.4],[-1,0,0,0.6],[0,0,0,1]]) xyz_targ = Tgoal[:3,3] quat_targ = rave.quatFromRotationMatrix(Tgoal[:3,:3]) request = move_arm_to_grasp(xyz_targ, quat_targ, LINK_NAME, MANIP_NAME) path_init = np.load("../data/barrett_traj3.npy") robot.SetActiveDOFValues(path_init[0,:]) s = json.dumps(request) print "REQUEST:",s trajoptpy.SetInteractive(args.interactive) prob = trajoptpy.ConstructProblem(s, env) result = trajoptpy.OptimizeProblem(prob) #stat = trajoptpy.SimulateAndReplan(s, env, 4) # stats = np.zeros((8,100))
manip = robot.GetManipulator(MANIP_NAME) viewer = trajoptpy.GetViewer(env) viewer.SetCameraTransformation([0,0,20], [0,0,0], [0,1,0]) ################## T_gripper = np.eye(4) T_gripper[:3,3] = np.array([2,2,0]) robot.GetLink(LINK_NAME).SetTransform(T_gripper) T_grasp = np.eye(4) xyz_targ = T_grasp[:3,3] # mk.create_mesh_box(env, np.array([5+0.01,0,0]), np.array([0.01,10,0.01])) mk.create_mesh_box(env, np.array([3.5,3,0]), np.array([1,1,2]), "box1") mk.create_mesh_box(env, np.array([3.5,-0.5,0]), np.array([1,1,2]), "box2") # mk.create_mesh_box(env, np.array([3.5,1.25,0]), np.array([0.2,0.2,1]), "box3") # mk.create_mesh_box(env, np.array([3.5,2,0]), np.array([0.2,0.2,1]), "box4") quat_targ = rave.quatFromRotationMatrix(T_grasp[:3,:3]) request = move_arm_to_grasp(xyz_targ, quat_targ, LINK_NAME, MANIP_NAME) ################## ''' # first optimize ignoring collision costs all_costs = request["costs"] noncollision_costs = [cost for cost in request["costs"] if "collision" not in cost["type"]] request["costs"] = noncollision_costs saver = rave.Robot.RobotStateSaver(robot) s = json.dumps(request) print "REQUEST:",s trajoptpy.SetInteractive(False);
def PlanToEndEffectorOffset(self, robot, direction, distance, max_distance=None, position_tolerance=0.01, angular_tolerance=0.15, **kwargs): """ Plan to an end-effector offset with a move-hand-straight constraint. This function plans a trajectory to purely translate a hand in the given direction as far as possible before collision. Movement less than min_distance will return failure. The motion will not also move further than max_distance. @param robot the robot whose active DOFs will be used @param direction unit vector in the direction of motion @param distance minimum distance in meters @param max_distance maximum distance in meters @param position_tolerance constraint tolerance in meters @param angular_tolerance constraint tolerance in radians @return traj a trajectory following specified direction """ # Plan using the active manipulator. with robot.GetEnv(): manipulator = robot.GetActiveManipulator() pose = manipulator.GetEndEffectorTransform() # Convert IK endpoint transformation to pose. ee_position = pose[0:3, 3].tolist() ee_rotation = openravepy.quatFromRotationMatrix(pose).tolist() # Create dummy frame that has direction vector along Z. z = direction y = numpy.cross(z, pose[0:3, 0]) # Cross with EE-frame Y. y /= numpy.linalg.norm(y) x = numpy.cross(y, z) cost_frame = numpy.vstack((x, y, z)).T cost_rotation = openravepy.quatFromRotationMatrix(cost_frame).tolist() # Construct a planning request with these constraints. n_steps = 10 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] }, }, { "type": "pose", "params": { "xyz": [0, 0, 0], "wxyz": cost_rotation, "link": manipulator.GetEndEffector().GetName(), "rot_coeffs": [0, 0, 0], "pos_coeffs": [0, 0, 10] } } ], "constraints": [ { "type": "pose", "params": { "xyz": ee_position, "wxyz": ee_rotation, "link": manipulator.GetEndEffector().GetName(), # "first_step": 0, # "last_step": n_steps-1, } } ], "init_info": { "type": "stationary" } } # Set active DOFs to match active manipulator and plan. p = openravepy.KinBody.SaveParameters with robot.CreateRobotStateSaver(p.ActiveDOF): robot.SetActiveDOFs(manipulator.GetArmIndices()) return self._Plan(robot, request, **kwargs)
def RosPoseFromRaveMatrix(mat): out = gm.Pose() out.position = gm.Point(*mat[:3, 3]) qw, qx, qy, qz = openravepy.quatFromRotationMatrix(mat[:3, :3]) out.orientation = gm.Quaternion(qx, qy, qz, qw) return out
def quat_from_rot(rot): return quatFromRotationMatrix(rot)
def _PlanToIK(self, robot, pose, ranker=None, **kwargs): # Plan using the active manipulator. with robot.GetEnv(): manipulator = robot.GetActiveManipulator() # Distance from current configuration is default ranking. if ranker is None: from prpy.ik_ranking import NominalConfiguration ranker = NominalConfiguration(manipulator.GetArmDOFValues()) # Find initial collision-free IK solution. from openravepy import (IkFilterOptions, IkParameterization, IkParameterizationType) ik_param = IkParameterization( pose, IkParameterizationType.Transform6D) ik_solutions = manipulator.FindIKSolutions( ik_param, IkFilterOptions.CheckEnvCollisions) if not len(ik_solutions): raise PlanningError('No collision-free IK solution.') # Sort the IK solutions in ascending order by the costs returned by the # ranker. Lower cost solutions are better and infinite cost solutions # are assumed to be infeasible. scores = ranker(robot, ik_solutions) best_idx = numpy.argmin(scores) init_joint_config = ik_solutions[best_idx] # Convert IK endpoint transformation to pose. goal_position = pose[0:3, 3].tolist() goal_rotation = openravepy.quatFromRotationMatrix(pose).tolist() # Settings for TrajOpt num_steps = 10 # Construct a planning request with these constraints. request = { "basic_info": { "n_steps": num_steps, "manip": "active", "start_fixed": True }, "costs": [ { "type": "joint_vel", "params": {"coeffs": [1]} }, { "type": "collision", "params": { "coeffs": [20], "dist_pen": [0.025] }, } ], "constraints": [ { "type": "pose", "params": { "xyz": goal_position, "wxyz": goal_rotation, "link": manipulator.GetEndEffector().GetName(), "timestep": num_steps-1 } } ], "init_info": { "type": "straight_line", "endpoint": init_joint_config.tolist() } } # Set active DOFs to match active manipulator and plan. p = openravepy.KinBody.SaveParameters with robot.CreateRobotStateSaver(p.ActiveDOF): robot.SetActiveDOFs(manipulator.GetArmIndices()) return self._Plan(robot, request, **kwargs)
def _PlanToIK(self, robot, robot_checker, pose, ranker=None, **kwargs): # Plan using the active manipulator. manipulator = robot.GetActiveManipulator() # Distance from current configuration is default ranking. if ranker is None: from prpy.ik_ranking import NominalConfiguration ranker = NominalConfiguration(manipulator.GetArmDOFValues()) # Find initial collision-free IK solution. ik_param = IkParameterization( pose, IkParameterizationType.Transform6D) ik_solutions = manipulator.FindIKSolutions( ik_param, IkFilterOptions.CheckEnvCollisions) if not len(ik_solutions): # Identify collision and raise error. self._raiseCollisionErrorForPose(robot, robot_checker, pose) # Sort the IK solutions in ascending order by the costs returned by the # ranker. Lower cost solutions are better and infinite cost solutions # are assumed to be infeasible. scores = ranker(robot, ik_solutions) best_idx = numpy.argmin(scores) init_joint_config = ik_solutions[best_idx] # Convert IK endpoint transformation to pose. OpenRAVE operates on # GetEndEffectorTransform(), which is equivalent to: # # GetEndEffector().GetTransform() * GetLocalToolTransform() # link_pose = numpy.dot( pose, numpy.linalg.inv( manipulator.GetLocalToolTransform())) goal_position = link_pose[0:3, 3].tolist() goal_rotation = openravepy.quatFromRotationMatrix(link_pose).tolist() # Settings for TrajOpt num_steps = 10 # Construct a planning request with these constraints. request = { "basic_info": { "n_steps": num_steps }, "costs": [ { "type": "joint_vel", "params": {"coeffs": [1]} }, { "type": "collision", "params": { "coeffs": [20], "dist_pen": [0.025] }, } ], "constraints": [ { "type": "pose", "params": { "xyz": goal_position, "wxyz": goal_rotation, "link": manipulator.GetEndEffector().GetName(), "timestep": num_steps-1 } } ], "init_info": { "type": "straight_line", "endpoint": init_joint_config.tolist() } } p = openravepy.KinBody.SaveParameters with robot.CreateRobotStateSaver(p.ActiveDOF): robot.SetActiveDOFs(manipulator.GetArmIndices()) return self._Plan(robot, robot_checker, request, **kwargs)