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):
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 move_ee_delta(self, delta, set_pos=None, set_ori=None):
""" Use controller to move end effector by some delta.
Args:
delta (6f): delta position (dx, dy, dz) concatenated with delta orientation.
Orientation change is specified as an axis angle rotation from previous
orientation.
set_pos (3f): end effector position to maintain while changing orientation.
[x, y, z]. If not None, the delta for position is ignored.
set_ori (4f): end effector orientation to maintain while changing orientation
as a quaternion [qx, qy, qz, w]. If not None, any delta for orientation is ignored.
Notes:
To fix position or orientation, it is better to specify using the kwargs than
to use a 0 for the corresponding delta. This prevents any error from accumulating in
that dimension.
Examples:
Specify delta holding fixed orientation::
self.robot_interface.move_ee_delta([0.1, 0, 0, 0, 0, 0], set_pos=None, set_ori=[0, 0, 0, 1])
"""
self._check_control_arg('delta', delta, mandatory=True, length=6)
if self._check_control_arg('set_ori', set_ori, mandatory=False, length=4):
set_ori = T.quat2mat(set_ori)
self._check_control_arg('set_pos', set_pos, mandatory=False, length=3)
self._check_controller(["EEImpedance", "EEPosture"])
kwargs = {'delta': delta, 'set_pos': set_pos, 'set_ori': set_ori}
self.set_controller_goal(**kwargs)
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 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 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 set_ee_pose(self, set_pos, set_ori, **kwargs):
""" Use controller to set end effector pose.
Args:
set_pos (list): 3f [x, y , z] desired absolute ee position
in world frame.
set_ori (list): 4f [qx, qy, qz, w] desired absolute ee orientation
in world frame.
**kwargs (dict): used to catch all other arguments.
Returns:
None
Notes: Only for use with EEImpedance and EEPosture controller.
"""
if self._check_control_arg('set_ori', set_ori, mandatory=True, length=4):
set_ori = T.quat2mat(set_ori)
self._check_control_arg('set_pos', set_pos, mandatory=True, length=3)
self._check_controller(["EEImpedance", "EEPosture"])
kwargs = {'delta': None, 'set_pos': set_pos, 'set_ori': set_ori}
self.set_controller_goal(**kwargs)
def update_model(self):
self._calc_jacobian()
self._calc_mass_matrix()
# Reset pybullet model to joint State to get ee_pose and orientation.
for joint_ind, joint_name in enumerate(self._joint_names):
pb.resetJointState(bodyUniqueId=self._pb_sawyer,
jointIndex=self.joint_dict[joint_name],
targetValue=self.q[joint_ind],
targetVelocity=self.dq[joint_ind],
physicsClientId=self._clid)
pb_ee_pos, pb_ee_ori, _, _, _, _, pb_ee_v, pb_ee_w = pb.getLinkState(
self._pb_sawyer,
self._link_id_dict[self.ee_name],
computeLinkVelocity=1,
computeForwardKinematics=1,
physicsClientId=self._clid)
# Get ee orientation as a matrix
ee_ori_mat = T.quat2mat(self.ee_orientation)
# Get end effector velocity in world frame
ee_v_world = np.dot(ee_ori_mat, np.array(self.ee_v).transpose())
ee_w_world = np.dot(ee_ori_mat, np.array(self.ee_omega).transpose())
self.ee_omega_world = ee_w_world
self.model.update_states(ee_pos=np.asarray(self.ee_position),
ee_ori=np.asarray(self.ee_orientation),
ee_pos_vel=np.asarray(ee_v_world),
ee_ori_vel=np.asarray(ee_w_world),
joint_pos=np.asarray(self.q[:7]),
joint_vel=np.asarray(self.dq[:7]),
joint_tau=np.asarray(self.tau),
joint_dim=7,
torque_compensation=self.torque_compensation)
self.model.update_model(J_pos=self.linear_jacobian,
J_ori=self.angular_jacobian,
mass_matrix=self.mass_matrix)
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
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))
def set_goal_orientation(delta,
current_orientation,
orientation_limit=None,
set_ori=None,
axis_angle=True):
"""
Calculates and returns the desired goal orientation, clipping the result accordingly to @orientation_limits.
@delta and @current_orientation must be specified if a relative goal is requested, else @set_ori must be
an orientation matrix specified to define a global orientation
If @axis_angle is set to True, then this assumes the input in axis angle form, that is,
a scaled axis angle 3-array [ax, ay, az]
"""
# directly set orientation
if set_ori is not None:
goal_orientation = set_ori
# otherwise use delta to set goal orientation
else:
if axis_angle:
# convert axis-angle value to rotation matrix
quat_error = trans.axisangle2quat(delta)
rotation_mat_error = trans.quat2mat(quat_error)
else:
# convert euler value to rotation matrix
rotation_mat_error = trans.euler2mat(-delta)
goal_orientation = np.dot(rotation_mat_error.T, current_orientation)
# check for orientation limits
if np.array(orientation_limit).any():
if orientation_limit.shape != (2,3):
raise ValueError("Orientation limit should be shaped (2,3) "
"but is instead: {}".format(orientation_limit.shape))
# Convert to euler angles for clipping
euler = trans.mat2euler(goal_orientation)
# Clip euler angles according to specified limits
limited = False
for idx in range(3):
if orientation_limit[0][idx] < orientation_limit[1][idx]: # Normal angle sector meaning
if orientation_limit[0][idx] < euler[idx] < orientation_limit[1][idx]:
continue
else:
limited = True
dist_to_lower = euler[idx] - orientation_limit[0][idx]
if dist_to_lower > np.pi:
dist_to_lower -= 2 * np.pi
elif dist_to_lower < -np.pi:
dist_to_lower += 2 * np.pi
dist_to_higher = euler[idx] - orientation_limit[1][idx]
if dist_to_lower > np.pi:
dist_to_higher -= 2 * np.pi
elif dist_to_lower < -np.pi:
dist_to_higher += 2 * np.pi
if dist_to_lower < dist_to_higher:
euler[idx] = orientation_limit[0][idx]
else:
euler[idx] = orientation_limit[1][idx]
else: # Inverted angle sector meaning
if (orientation_limit[0][idx] < euler[idx]
or euler[idx] < orientation_limit[1][idx]):
continue
else:
limited = True
dist_to_lower = euler[idx] - orientation_limit[0][idx]
if dist_to_lower > np.pi:
dist_to_lower -= 2 * np.pi
elif dist_to_lower < -np.pi:
dist_to_lower += 2 * np.pi
dist_to_higher = euler[idx] - orientation_limit[1][idx]
if dist_to_lower > np.pi:
dist_to_higher -= 2 * np.pi
elif dist_to_lower < -np.pi:
dist_to_higher += 2 * np.pi
if dist_to_lower < dist_to_higher:
euler[idx] = orientation_limit[0][idx]
else:
euler[idx] = orientation_limit[1][idx]
if limited:
goal_orientation = trans.euler2mat(np.array([euler[1], euler[0], euler[2]]))
return goal_orientation
