def _compile_jit_functions(self):
dummy_mat = np.eye(3)
dummy_quat = np.zeros(4)
dummy_quat[-1] = 1.
T.mat2quat(dummy_mat)
T.quat2mat(dummy_quat)
dummy_J = np.zeros((6, 7))
dummy_dq = np.zeros(7)
T.calc_twist(dummy_J, dummy_dq)
def apply_delta(pose, delta):
""" Applies delta to pose to obtain new 7f pose.
Args: pose (7f): x, y, z, qx, qy, qz, w . Position and quaternion
delta (6f): dx, dy, dz, ax, ay, az. delta position and axis-angle
"""
if len(delta) != 6:
raise ValueError(
"delta should be [x, y, z, ax, ay, az]. Orientation should be axis-angle"
)
if len(pose) != 7:
raise ValueError(
"pose should be [x, y, z, qx, qy, qz, w] Orientation should be quaternion."
)
pos = pose[:3]
dpos = delta[:3]
# Get current orientation and delta as matrices to apply 3d rotation.
ori_mat = T.quat2mat(pose[3:])
delta_quat = T.axisangle2quat(delta[3:])
delta_mat = T.quat2mat(delta_quat)
new_ori = np.dot(delta_mat.T, ori_mat)
# convert new orientation to quaternion.
new_ori_quat = T.mat2quat(new_ori)
# add dpos to current position.
new_pos = np.add(pos, dpos)
return np.hstack((new_pos, new_ori_quat))
def _compile_jit_functions(self):
"""
Helper function to incur the cost of compiling jit functions.
"""
dummy_mat = np.eye(3)
dummy_quat = np.zeros(4)
dummy_quat[-1] = 1.
T.mat2quat(dummy_mat)
T.quat2mat(dummy_quat)
_, _, _, dummy_nullspace_matrix = C.opspace_matrices(
mass_matrix=self.model.mass_matrix,
J_full=self.model.J_full,
J_pos=self.model.J_pos,
J_ori=self.model.J_ori,
)
C.orientation_error(dummy_mat, dummy_mat)
def _compile_jit_functions(self):
dummy_mat = np.eye(3)
dummy_quat = np.zeros(4)
dummy_quat[-1] = 1.
T.mat2quat(dummy_mat)
T.quat2mat(dummy_quat)
_, _, _, dummy_nullspace_matrix = C.opspace_matrices(
mass_matrix=self.model.mass_matrix,
J_full=self.model.J_full,
J_pos=self.model.J_pos,
J_ori=self.model.J_ori,
)
C.orientation_error(dummy_mat, dummy_mat)
C.nullspace_torques(
mass_matrix=self.model.mass_matrix,
nullspace_matrix=dummy_nullspace_matrix,
initial_joint=self.goal_posture,
joint_pos=self.model.joint_pos,
joint_vel=self.model.joint_vel,
)
def update_states(self,
ee_pos,
ee_ori,
ee_pos_vel,
ee_ori_vel,
joint_pos,
joint_vel,
joint_tau,
joint_dim=None,
torque_compensation=None):
self.ee_pos = ee_pos
if ee_ori.shape == (3, 3):
self.ee_ori_mat = ee_ori
self.ee_ori_quat = T.mat2quat(ee_ori)
elif ee_ori.shape[0] == 4:
self.ee_ori_quat = ee_ori
self.ee_ori_mat = T.quat2mat(ee_ori)
else:
raise ValueError("orientation is not quaternion or matrix")
self.ee_pos_vel = ee_pos_vel
self.ee_ori_vel = ee_ori_vel
self.joint_pos = joint_pos
self.joint_vel = joint_vel
self.joint_tau = joint_tau
# Only update the joint_dim and torque_compensation attributes if it hasn't been updated in the past
if not self.joint_dim:
if joint_dim is not None:
# User has specified explicit joint dimension
self.joint_dim = joint_dim
else:
# Default to joint_pos length
self.joint_dim = len(joint_pos)
# Update torque_compensation accordingly
#self.torque_compensation = np.zeros(self.joint_dim)
if torque_compensation is None:
self.torque_compensation = np.zeros(7)
else:
self.torque_compensation = np.asarray(torque_compensation)
def get_delta(self, goal_pose, current_pose):
"""Get delta between goal pose and current_pose.
Args: goal_pose (list): 7f pose [x, y, z, qx, qy, qz, w] . Position and quaternion of goal pose.
current_pose (list): 7f Position and quaternion of current pose.
Returns: delta (list) 6f [dx, dy, dz, ax, ay, az] delta position and delta orientation
as axis-angle.
"""
if len(goal_pose) != 7:
raise ValueError("Goal pose incorrect dimension should be 7f")
if len(current_pose) != 7:
raise ValueError("Current pose incorrect dimension, should be 7f")
dpos = np.subtract(goal_pose[:3], current_pose[:3])
goal_mat = T.quat2mat(goal_pose[3:])
current_mat = T.quat2mat(current_pose[3:])
delta_mat_T = np.dot(goal_mat, current_mat.T)
delta_quat = T.mat2quat(np.transpose(delta_mat_T))
delta_aa = T.quat2axisangle(delta_quat)
return np.hstack((dpos, delta_aa)).tolist()
def set_goal(self, delta, set_pos=None, set_ori=None, **kwargs):
""" Set goal for controller.
Args:
delta (list): (6f) list of deltas from current position
[dx, dy, dz, ax, ay, az]. Deltas expressed as position and
axis-angle in orientation.
set_pos (list): (3f) fixed position goal. [x, y, z] in world
frame. Only used if delta is None.
set_ori (list): (4f) fixed orientation goal as quaternion in
world frame. Only used if delta is None.
kwargs (dict): additional keyword arguments.
Returns:
None
Examples::
$ self.controller.set_goal(delta=[0.1, 0.1, 0, 0, 0, 0], set_pos=None, set_ori=None)
$ self.controller.set_goal(delta=None, set_pose=[0.4, 0.2, 0.4], set_ori=[0, 0, 0, 1])
"""
# Check args for dims and type (change to ndarray).
if not (isinstance(set_pos, np.ndarray)) and set_pos is not None:
set_pos = np.array(set_pos)
if not (isinstance(set_ori, np.ndarray)) and set_ori is not None:
if len(set_ori) != 4:
raise ValueError(
"invalid ori dimensions, should be quaternion.")
else:
set_ori = T.quat2mat(np.array(set_ori))
# Update the model.
self.model.update()
# If using a delta. Scale delta and set position and orientation goals.
if delta is not None:
if (len(delta) < 6):
raise ValueError("incorrect delta dimension")
scaled_delta = self.scale_action(delta)
# print("scaled_delta {}".format(delta))
self.goal_ori = C.set_goal_orientation(
scaled_delta[3:],
self.model.ee_ori_mat,
orientation_limit=self.orientation_limits,
set_ori=set_ori,
axis_angle=True)
self.goal_pos = C.set_goal_position(
scaled_delta[:3],
self.model.ee_pos,
position_limit=self.position_limits,
set_pos=set_pos)
else:
# If goal position and orientation are set.
scaled_delta = None
self.goal_ori = C.set_goal_orientation(
None,
self.model.ee_ori_mat,
orientation_limit=self.orientation_limits,
set_ori=set_ori,
axis_angle=True)
self.goal_pos = C.set_goal_position(
None,
self.model.ee_pos,
position_limit=self.position_limits,
set_pos=set_pos)
# Set goals for interpolators.
if self.interpolator_pos is not None:
self.interpolator_pos.set_goal(self.goal_pos)
if self.interpolator_ori is not None:
self.interpolator_ori.set_goal(T.mat2quat(self.goal_ori))