def _get_pose_com_(self, frame):
com_numerator = np.array([0.0, 0.0, 0.0])
for link_index in range(p.getNumJoints(self.id)):
link_com = p.getLinkState(self.id, link_index)[0]
link_mass = p.getDynamicsInfo(self.id, link_index)[0]
com_numerator = np.add(com_numerator,
np.multiply(link_mass, link_com))
p_com_world = np.divide(com_numerator, self._mass)
p_base_world, q_base_world = p.getBasePositionAndOrientation(self.id)
q_com_world = q_base_world
if frame == 'world':
return p_com_world, q_com_world
elif frame == 'tip':
p_tip_world = self._get_p_tip_world()
p_com_tip = util.transformation(p_com_world,
p_tip_world,
q_base_world,
inverse=True)
q_com_tip = np.array([0., 0., 0., 1.])
return p_com_tip, q_com_tip
elif frame == 'base':
p_com_base = util.transformation(p_com_world,
p_base_world,
q_base_world,
inverse=True)
q_com_base = np.array([0.0, 0.0, 0.0, 1.0])
return p_com_base, q_com_base
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 _get_p_tip_base(self):
p_base_world, q_base_world = p.getBasePositionAndOrientation(self.id)
p_tip_world = self._get_p_tip_world()
p_tip_base = util.transformation(p_tip_world,
p_base_world,
q_base_world,
inverse=True)
return p_tip_base
def _set_pose_tip_world(self, pose_tip_world_des, reset=False):
p_base_tip = np.multiply(-1, self._get_p_tip_base())
p_base_world_des = util.transformation(p_base_tip,
pose_tip_world_des.p,
pose_tip_world_des.q)
p.resetBasePositionAndOrientation(self.id, p_base_world_des,
pose_tip_world_des.q)
p.stepSimulation()
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 _inverse_kinematics(self, p_joint_world, q_joint_world):
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)
M_joint_world = util.pose_to_matrix(p_joint_world, q_joint_world)
M_world_origin = np.linalg.inv(self._M_origin_world)
M_joint_origin = np.dot(M_world_origin, M_joint_world)
p_joint_origin = M_joint_origin[:3, 3]
return np.dot(prismatic_dir, p_joint_origin)
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 _move_PD(self,
pose_handle_base_world_des,
q_offset,
mech,
last_traj_p,
debug=False,
stable_timeout=100,
unstable_timeout=1000):
finished = False
handle_base_ps = []
for i in itertools.count():
handle_base_ps.append(mech.get_pose_handle_base_world().p)
if self.use_gripper:
self._control_fingers('close', debug=debug)
if (not last_traj_p) and self._at_des_handle_base_pose(
pose_handle_base_world_des, q_offset, mech, 0.01):
return handle_base_ps, False
elif last_traj_p and self._at_des_handle_base_pose(
pose_handle_base_world_des, q_offset, mech,
0.000001) and self._stable(handle_base_ps):
return handle_base_ps, True
elif self._stable(handle_base_ps) and (i > stable_timeout):
return handle_base_ps, True
elif i > unstable_timeout:
return handle_base_ps, True
# get position error of the handle base
p_handle_base_world_err, e_handle_base_world_err = self._get_pose_handle_base_world_error(
pose_handle_base_world_des, q_offset, mech)
# use handle vel or gripper vel to calc velocity error
if not self.use_gripper:
lin_v_com_world_err = p.getLinkState(mech._get_bb_id(), \
mech._get_handle_id(),
computeLinkVelocity=1)[6]
else:
v_tip_world_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
lin_v_com_world_err, omega_com_world_err = self._get_v_com_world_error(
v_tip_world_des)
f = np.multiply(self.k[0], p_handle_base_world_err) + \
np.multiply(self.d[0], lin_v_com_world_err)
# only apply torques if using gripper
if self.use_gripper:
tau = np.multiply(
self.k[1], e_handle_base_world_err
) #+ np.multiply(self.d[1], omega_com_world_err)
else:
tau = [0, 0, 0]
self.errors += [(p_handle_base_world_err, lin_v_com_world_err)]
self.forces += [(f, tau)]
if not self.use_gripper:
bb_id = mech._get_bb_id()
handle_id = mech._get_handle_id()
handle_pos = mech.get_pose_handle_base_world().p
handle_q = mech.get_pose_handle_base_world().q
# transform the force into the LINK_FRAME to apply
f = util.transformation(f, [0., 0., 0.],
handle_q,
inverse=True)
p.applyExternalForce(bb_id, handle_id, f, [0., 0., 0.],
p.LINK_FRAME)
if debug:
p.addUserDebugLine(handle_pos,
np.add(handle_pos,
2 * (f / np.linalg.norm(f))),
lifeTime=.05)
else:
p_com_world, q_com_world = self._get_pose_com_('world')
p.applyExternalForce(self.id, -1, f, p_com_world,
p.WORLD_FRAME)
# there is a bug in pyBullet. the link frame and world frame are inverted
# this should be executed in the WORLD_FRAME
p.applyExternalTorque(self.id, -1, tau, p.LINK_FRAME)
p.stepSimulation()
def _gen(mech, x_dict={}):
""" This function generates a Revolute policy. The ranges are
based on the data.generator range revolute joints
"""
param_data = Policy.get_param_data('Revolute')
# axis roll
if param_data['rot_axis_roll'].varied:
if 'rot_axis_roll' in x_dict:
rot_axis_roll_world = x_dict['rot_axis_roll']
else:
rot_axis_roll_world = \
np.random.uniform(*param_data['rot_axis_roll'].bounds)
else:
rot_axis_roll_world = 0.0
# axis pitch
if param_data['rot_axis_pitch'].varied:
if 'rot_axis_pitch' in x_dict:
rot_axis_pitch_world = x_dict['rot_axis_pitch']
else:
rot_axis_pitch_world = \
np.random.uniform(*param_data['rot_axis_pitch'].bounds)
else:
if mech.mechanism_type == 'Door':
if not mech.flipped:
rot_axis_pitch_world = np.pi
else:
rot_axis_pitch_world = 0.0
else:
raise Exception(
'Cannot use ground truth rot_axis_pitch for Revolute \
policies on non-Revolute mechanisms as there is \
not a single ground truth value. It varies with \
each Revolute mechanism. Must vary rot_axis_pitch \
param for non-Revolute mechanism.')
# axis yaw
if param_data['rot_axis_yaw'].varied:
if 'rot_axis_yaw' in x_dict:
rot_axis_yaw_world = x_dict['rot_axis_yaw']
else:
rot_axis_yaw_world = \
np.random.uniform(*param_data['rot_axis_yaw'].bounds)
else:
rot_axis_yaw_world = 0.0
# radius_x
if param_data['radius_x'].varied:
if 'radius_x' in x_dict:
radius_x = x_dict['radius_x']
else:
radius_x = np.random.uniform(*param_data['radius_x'].bounds)
else:
if mech.mechanism_type == 'Door':
radius_x = mech.get_radius_x()
else:
raise Exception(
'Cannot use ground truth radius_x for Revolute \
policies on non-Revolute mechanisms as there is \
not a single ground truth value. It varies with \
each Revolute mechanism. Must vary radius_x \
param for non-Revolute mechanism.')
# center of rotation
rot_axis_world = util.quaternion_from_euler(rot_axis_roll_world,
rot_axis_pitch_world,
rot_axis_yaw_world)
radius = [-radius_x, 0.0, 0.0]
p_handle_base_world = mech.get_pose_handle_base_world().p
p_rot_center_world = p_handle_base_world + util.transformation(
radius, [0., 0., 0.], rot_axis_world)
# goal config
if param_data['goal_config'].varied:
if 'goal_config' in x_dict:
goal_config = x_dict['goal_config']
else:
goal_config = np.random.uniform(
*param_data['goal_config'].bounds)
else:
if mech.mechanism_type == 'Door':
goal_config = -np.pi / 2
else:
raise Exception('Cannot use ground truth config for Revolute \
policies on non-Revolute mechanisms as there is \
not a single ground truth value. It varies with \
each Revolute mechanism. Must vary config \
param for non-Revolute mechanism.')
return Revolute(p_rot_center_world, rot_axis_roll_world,
rot_axis_pitch_world, rot_axis_yaw_world, radius_x,
goal_config, param_data)
