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