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