def q_tilde_quat(self, q, target): """Compute the error signal when there are quaternions in the state space and target signals. If there are quaternions in the state space, calculate the error and then transform them to 3D space for the control signal. NOTE: Assumes that for every quaternion there are 3 motors, that effect movement along the x, y, and z axes, applied in that order. Parameters ---------- q : float numpy.array mix of joint angles and quaternions [quaternions & radians] target : float numpy.array mix of target joint angles and quaternions [quaternions & radians] """ # for each quaternion in the list, the output size is reduced by 1 q_tilde = np.zeros(len(q) - np.sum(self.quaternions)) q_index = 0 q_tilde_index = 0 for quat_bool in self.quaternions: if quat_bool: # if the joint position is a quaternion, calculate error joint_q = q[q_index:q_index + 4] error = quaternion_multiply( target[q_index:q_index + 4], quaternion_conjugate(joint_q), ) # NOTE: we need to rotate this back to undo the current angle # because the actuators don't rotate with the ball joint u = quaternion_multiply( quaternion_conjugate(joint_q), quaternion_multiply( error, joint_q, ), ) # convert from quaternion to 3D angular forces to apply # https://studywolf.wordpress.com/2018/12/03/force-control-of-task-space-orientation/ q_tilde[q_tilde_index:q_tilde_index + 3] = u[1:] * np.sign(u[0]) q_index += 4 q_tilde_index += 3 else: q_tilde[q_tilde_index] = self.q_tilde_angle( q[q_index], target[q_index]) q_index += 1 q_tilde_index += 1 return q_tilde
def _calc_orientation_forces(self, target_abg, q): """Calculate the desired Euler angle forces to apply to the arm to move the end-effector to the target orientation Parameters ---------- target_abg: np.array the target Euler angles orientation for the end-effector: alpha, beta, gamma q: np.array the joint angles of the arm """ u_task_orientation = np.zeros(3) if self.orientation_algorithm == 0: # transform the orientation target into a quaternion q_d = transformations.unit_vector( transformations.quaternion_from_euler( target_abg[0], target_abg[1], target_abg[2], axes="rxyz" ) ) # get the quaternion for the end effector q_e = self.robot_config.quaternion("EE", q=q) q_r = transformations.quaternion_multiply( q_d, transformations.quaternion_conjugate(q_e) ) u_task_orientation = -q_r[1:] * np.sign(q_r[0]) elif self.orientation_algorithm == 1: # From (Caccavale et al, 1997) Section IV Quaternion feedback # get rotation matrix for the end effector orientation R_e = self.robot_config.R("EE", q) # get rotation matrix for the target orientation R_d = transformations.euler_matrix( target_abg[0], target_abg[1], target_abg[2], axes="rxyz" )[:3, :3] R_ed = np.dot(R_e.T, R_d) # eq 24 q_ed = transformations.unit_vector( transformations.quaternion_from_matrix(R_ed) ) u_task_orientation = -1 * np.dot(R_e, q_ed[1:]) # eq 34 else: raise Exception( "Invalid algorithm number %i for calculating " % self.orientation_algorithm + "orientation error" ) return u_task_orientation
def test_calc_orientation_forces(arm, orientation_algorithm): robot_config = arm.Config(use_cython=False) ctrlr = OSC(robot_config, orientation_algorithm=orientation_algorithm) for ii in range(100): q = np.random.random(robot_config.N_JOINTS) * 2 * np.pi quat = robot_config.quaternion("EE", q=q) theta = np.pi / 2 axis = np.array([0, 0, 1]) quat_rot = transformations.unit_vector( np.hstack([np.cos(theta / 2), np.sin(theta / 2) * axis])) quat_target = transformations.quaternion_multiply(quat, quat_rot) target_abg = transformations.euler_from_quaternion(quat_target, axes="rxyz") # calculate current position quaternion R = robot_config.R("EE", q=q) quat_1 = transformations.unit_vector( transformations.quaternion_from_matrix(R)) dist1 = calc_distance(quat_1, np.copy(quat_target)) # calculate current position quaternion with u_task added u_task = ctrlr._calc_orientation_forces(target_abg, q=q) dq = np.dot(np.linalg.pinv(robot_config.J("EE", q)), np.hstack([np.zeros(3), u_task])) q_2 = q - dq * 0.001 # where 0.001 represents the time step R_2 = robot_config.R("EE", q=q_2) quat_2 = transformations.unit_vector( transformations.quaternion_from_matrix(R_2)) dist2 = calc_distance(quat_2, np.copy(quat_target)) assert np.abs(dist2) < np.abs(dist1)
# calculate position error u_task[:3] = -kp * (hand_xyz - target[:3]) # Method 1 ------------------------------------------------------------ # # transform the orientation target into a quaternion q_d = transformations.unit_vector( transformations.quaternion_from_euler( target[3], target[4], target[5], axes='rxyz')) # get the quaternion for the end effector q_e = transformations.unit_vector( transformations.quaternion_from_matrix( robot_config.R('EE', feedback['q']))) # calculate the rotation from the end-effector to target orientation q_r = transformations.quaternion_multiply( q_d, transformations.quaternion_conjugate(q_e)) u_task[3:] = q_r[1:] * np.sign(q_r[0]) # Method 2 ------------------------------------------------------------ # From (Caccavale et al, 1997) Section IV - Quaternion feedback ------- # # get rotation matrix for the end effector orientation # R_EE = robot_config.R('EE', feedback['q']) # # get rotation matrix for the target orientation # R_d = transformations.euler_matrix( # target[3], target[4], target[5], axes='rxyz')[:3, :3] # R_ed = np.dot(R_EE.T, R_target) # eq 24 # q_e = transformations.quaternion_from_matrix(R_ed) # q_e = transformations.unit_vector(q_e) # u_task[3:] = np.dot(R_EE, q_e[1:]) # eq 34
u = ctrlr.generate( q=feedback["q"], dq=feedback["dq"], target=target, ) # send forces into Mujoco, step the sim forward interface.send_forces(u) # track data q_track.append(np.copy(feedback["q"])) target_track.append(np.copy(target)) # calculate the distance between quaternions error = quaternion_multiply( target, quaternion_conjugate(feedback["q"]), ) error = 2 * np.arctan2( np.linalg.norm(error[1:]) * -np.sign(error[0]), error[0]) # quaternion distance for same angles can be 0 or 2*pi, so use a sine # wave here so 0 and 2*np.pi = 0 error = np.sin(error / 2) error_track.append(np.copy(error)) if abs(error) < threshold: interface.sim.model.geom_rgba[target_geom_id] = green count += 1 else: count = 0 interface.sim.model.geom_rgba[target_geom_id] = red
dq=feedback["dq"], target=target, ) # send forces into Mujoco, step the sim forward interface.send_forces(u) # track data q_track.append(np.copy(feedback["q"])) target_track.append(np.copy(target)) # calculate the distance between quaternions error = 0.0 for ii in range(2): temp = quaternion_multiply( target[ii * 4:(ii * 4) + 4], quaternion_conjugate(feedback["q"][ii * 4:(ii * 4) + 4]), ) temp = 2 * np.arctan2( np.linalg.norm(temp[1:]) * -np.sign(temp[0]), temp[0]) # quaternion distance for same angles can be 0 or 2*pi, so use a sine # wave here so 0 and 2*np.pi = 0 error += np.sin(temp / 2) error_track.append(np.copy(error)) if abs(error) < threshold: interface.sim.model.geom_rgba[target_geom_id] = green count += 1 else: count = 0 interface.sim.model.geom_rgba[target_geom_id] = red