def get_pose_handle_base_world(self): handle_id = self._get_handle_id() bb_id = self._get_bb_id() pose_handle_world = util.Pose(*p.getLinkState(bb_id, handle_id)[:2]) p_handle_base = [0., 0., self.handle_length / 2] p_handle_base_world = util.transformation(p_handle_base, *pose_handle_world) return util.Pose(p_handle_base_world, pose_handle_world.q)
def __init__(self, mech, k=[2000.0, 20.0], d=[0.45, 0.45]): """ This class defines the actions a gripper can take such as grasping a handle and executing PD control :param mech: the gen.generator_busybox.Mechanism being actuated :param k: a vector of length 2 where the first entry is the linear position (stiffness) gain and the second entry is the angular position gain :param d: a vector of length 2 where the first entry is the linear derivative (damping) gain and the second entry is the angular derivative gain """ self.use_gripper = False if self.use_gripper: self.id = p.loadSDF("models/gripper/gripper_high_fric.sdf")[0] self._left_finger_tip_id = 2 self._right_finger_tip_id = 5 self._left_finger_base_joint_id = 0 self._right_finger_base_joint_id = 3 self._finger_force = 20 self.pose_tip_world_reset = util.Pose([0.0, 0.0, 0.2], \ [0.50019904, 0.50019904, -0.49980088, 0.49980088]) # get mass of gripper mass = 0 for link in range(p.getNumJoints(self.id)): mass += p.getDynamicsInfo(self.id, link)[0] self._mass = mass self.errors = [] self.forces = [] # control parameters self.k = k self.d = d
def __init__(self, pos, orn, pitch, yaw, goal_config, param_data): """ :param pos: vector of length 3, a rigid (x,y,z) position in the world frame along the prismatic joint :param orn: vector of length 4, a rigid (x,y,z,w) quaternion in the world frame representing the orientation of the handle in the world :param pitch: scalar, pitch between world frame and the direction of the prismatic joint :param yaw: scalar, yaw between world frame and the direction of the prismatic joint :param goal_config: scalar, distance to move along constrained trajectory :param param_delta: dict, keys are param names and values are ParamData tuples """ self.rigid_position = pos self.rigid_orientation = orn self.pitch = pitch self.yaw = yaw self.goal_config = goal_config self.param_data = param_data # derived self._M_origin_world = util.pose_to_matrix(self.rigid_position, self.rigid_orientation) self.a = util.Pose(self.rigid_position, self.rigid_orientation) q_prismatic_dir = util.quaternion_from_euler(0.0, self.pitch, self.yaw) self.e = util.transformation([1., 0., 0.], [0., 0., 0.], q_prismatic_dir) super(Prismatic, self).__init__('Prismatic')
def _forward_kinematics(self, config): # rotation matrix for a rotation about the z-axis by config radians M_joint_z = util.trans.rotation_matrix(-config, [0, 0, 1]) M_joint_world = util.trans.concatenate_matrices( self._M_center_world, M_joint_z, self._M_radius_center) p_joint_world = M_joint_world[:3, 3] q_joint_world = util.quaternion_from_matrix(M_joint_world) return util.Pose(p_joint_world, q_joint_world)
def _forward_kinematics(self, config): q_prismatic_dir = util.quaternion_from_euler(0.0, self.pitch, self.yaw) prismatic_dir = util.transformation([1., 0., 0.], [0., 0., 0.], q_prismatic_dir) p_joint_origin = np.multiply(config, prismatic_dir) p_joint_origin_4 = np.concatenate([p_joint_origin, [1.]]) p_joint_world = np.dot(self._M_origin_world, p_joint_origin_4)[:3] q_joint_world = util.quaternion_from_matrix(self._M_origin_world) return util.Pose(p_joint_world, q_joint_world)
def execute_trajectory(self, traj, mech, policy_type, debug): pose_handle_base_world_init = mech.get_pose_handle_base_world() self.set_control_params(policy_type) # offset between the initial trajectory orientation and the initial handle orientation q_offset = util.quat_math(traj[0].q, mech.get_pose_handle_base_world().q, True, False) if self.use_gripper: pose_handle_world_init = mech.get_handle_pose() p_tip_world_init = np.add( pose_handle_world_init.p, [0., .015, 0.]) # back up a little for better grasp pose_tip_world_init = util.Pose(p_tip_world_init, self.pose_tip_world_reset.q) self._grasp_handle(pose_tip_world_init, debug) cumu_motion = 0.0 for i in range(len(traj)): last_traj_p = (i == len(traj) - 1) handle_base_ps, finished = self._move_PD(traj[i], q_offset, mech, last_traj_p, debug) cumu_motion = np.add( cumu_motion, np.linalg.norm( np.subtract(handle_base_ps[-1], handle_base_ps[0]))) if finished: break pose_handle_world_final = None if not self.use_gripper or self._in_contact(mech): pose_handle_world_final = mech.get_handle_pose() net_motion = 0.0 if pose_handle_world_final is not None: pose_handle_base_world_final = mech.get_pose_handle_base_world() net_motion = np.linalg.norm(np.subtract(pose_handle_base_world_final.p, \ pose_handle_base_world_init.p)) #self.plot_err_forces() return cumu_motion, net_motion, pose_handle_world_final
def get_handle_pose(self): handle_id = self._get_handle_id() bb_id = self._get_bb_id() return util.Pose(*p.getLinkState(bb_id, handle_id)[:2])