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 compute_error(self, goal_state, new_state):
""" Compute the error between current state and goal state.
For OpSpace Demos, this is position and orientation error.
"""
goal_pos = goal_state[:3]
new_pos = new_state[:3]
# Check or convert to correct orientation
goal_ori = T.convert_euler_quat_2mat(goal_state[3:])
new_ori = T.convert_euler_quat_2mat(new_state[3:])
pos_error = np.subtract(goal_pos, new_pos)
ori_error = C.orientation_error(goal_ori, new_ori)
return np.hstack((pos_error, ori_error))
def run_controller(self):
""" Calculate torques to acheive goal.
See controllers documentation for more information on EEImpedance
control. Set goal must be called before running controller.
Args:
None
Returns:
torques (list): 7f list of torques to command.
Run impedance controllers.
"""
# Check if the goal has been set.
if self.goal_pos is None or self.goal_ori is None:
raise ValueError("Set goal first.")
# Default desired velocities and accelerations are zero.
desired_vel_pos = np.asarray([0.0, 0.0, 0.0])
desired_acc_pos = np.asarray([0.0, 0.0, 0.0])
desired_vel_ori = np.asarray([0.0, 0.0, 0.0])
desired_acc_ori = np.asarray([0.0, 0.0, 0.0])
# Get interpolated goal for position
if self.interpolator_pos is not None:
desired_pos = self.interpolator_pos.get_interpolated_goal()
else:
desired_pos = np.array(self.goal_pos)
# Get interpolated goal for orientation.
if self.interpolator_ori is not None:
desired_ori = T.quat2mat(
self.interpolator_ori.get_interpolated_goal())
ori_error = C.orientation_error(desired_ori, self.model.ee_ori_mat)
else:
desired_ori = np.array(self.goal_ori)
ori_error = C.orientation_error(desired_ori, self.model.ee_ori_mat)
# Calculate desired force, torque at ee using control law and error.
position_error = desired_pos - self.model.ee_pos
# print("pos_error {}".format(position_error))
vel_pos_error = desired_vel_pos - self.model.ee_pos_vel
desired_force = (
np.multiply(np.array(position_error), np.array(self.kp[0:3])) +
np.multiply(vel_pos_error, self.kv[0:3])) + desired_acc_pos
vel_ori_error = desired_vel_ori - self.model.ee_ori_vel
desired_torque = (
np.multiply(np.array(ori_error), np.array(self.kp[3:])) +
np.multiply(vel_ori_error, self.kv[3:])) + desired_acc_ori
# Calculate Operational Space mass matrix and nullspace.
lambda_full, lambda_pos, lambda_ori, nullspace_matrix = \
C.opspace_matrices(self.model.mass_matrix,
self.model.J_full,
self.model.J_pos,
self.model.J_ori)
self.nullspace_matrix = nullspace_matrix
# If uncoupling position and orientation use separated lambdas.
if self.uncoupling:
decoupled_force = np.dot(lambda_pos, desired_force)
decoupled_torque = np.dot(lambda_ori, desired_torque)
decoupled_wrench = np.concatenate(
[decoupled_force, decoupled_torque])
else:
desired_wrench = np.concatenate([desired_force, desired_torque])
decoupled_wrench = np.dot(lambda_full, desired_wrench)
# Project torques that acheive goal into task space.
self.torques = np.dot(
self.model.J_full.T,
decoupled_wrench) + self.model.torque_compensation
return self.torques