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 # create a target orientation target_abg = np.random.random(3) * np.pi - np.pi / 2.0 # calculate current position quaternion R = robot_config.R('EE', q=q) quat_1 = transformations.unit_vector( transformations.quaternion_from_matrix(R)) # 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)) # calculate the quaternion of the target quat_target = transformations.unit_vector( transformations.quaternion_from_euler( target_abg[0], target_abg[1], target_abg[2], axes='rxyz')) dist1 = calc_distance(quat_1, np.copy(quat_target)) dist2 = calc_distance(quat_2, np.copy(quat_target)) assert np.abs(dist2) < np.abs(dist1)
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 generate_path( self, start_position, target_position, max_velocity, start_orientation=None, target_orientation=None, start_velocity=0, target_velocity=0, plot=False, ): """ Takes a start and target position, along with an optional start and target velocity, and generates a trajectory that smoothly accelerates, at a rate defined by vel_profile, from start_velocity to max_v, and back to target_v. If the path is too short to reach max_v and still decelerate to target_v at a rate of max_a, then the path will be slowed to the maximum allowable velocity so that we still reach target_velocity at the moment we are at target_position. Optionally can pass in a 3D angular state [a, b, g] and target orientation. Note that the orientation should be given in euler angles, in the ordered specified by axes on init. The orientation path will follow the same velocity profile as the position path. Parameters ---------- start_position: 3x1 np.array of floats The starting position (x, y, z). target_position: 3x1 np.array of floats The target position (x, y, z). max_velocity: float The maximum allowable velocity of the path. start_velocity: float, Optional (Default: 0) The velocity at start of path. target_velocity: float, Optional (Default: 0) The velocity at end of path. start_orientation: 3x1 np.array of floats, Optional (Default: None) The orientation at start of path in euler angles, given in the order specified on __init__ with the axes parameter (default rxyz). When left as `None`, no orientation path will be planned. target_orientation: 3x1 np.array of floats, Optional (Default: None) The target orientation at the end of the path in euler angles, given in the order specified on __init__ with the axes parameter. plot: bool, Optional (Default: False) Set `True` to plot path profiles for debugging. """ assert start_velocity <= max_velocity, ( f"{c.red}start velocity({start_velocity}m/s) " + f"> max velocity({max_velocity}m/s){c.endc}") assert target_velocity <= max_velocity, ( f"{c.red}target velocity({target_velocity}m/s) " + f"> max velocity({max_velocity}m/s){c.endc}") if start_velocity == max_velocity: self.starting_dist = 0 self.starting_vel_profile = [start_velocity * self.dt] else: self.starting_dist = None if target_velocity == max_velocity: self.ending_dist = 0 self.ending_vel_profile = [target_velocity * self.dt] else: self.ending_dist = None self.max_velocity = max_velocity if self.starting_dist is None: # Regenerate our velocity curves if start or end v have changed if (self.starting_vel_profile is None or self.start_velocity != start_velocity): self.starting_vel_profile = self.vel_profile.generate( start_velocity=start_velocity, target_velocity=self.max_velocity) # calculate the distance covered ramping from start_velocity to # max_v and from max_v to target_velocity self.starting_dist = np.sum(self.starting_vel_profile * self.dt) if self.ending_dist is None: # if our start and end v are the same, just mirror the curve to # avoid regenerating if start_velocity == target_velocity: self.ending_vel_profile = self.starting_vel_profile[::-1] # if target velocity is different, generate its unique curve elif (self.ending_vel_profile is None or self.target_velocity != target_velocity): self.ending_vel_profile = self.vel_profile.generate( start_velocity=target_velocity, target_velocity=self.max_velocity)[::-1] # calculate the distance covered ramping from start_velocity to # max_v and from max_v to target_velocity self.ending_dist = np.sum(self.ending_vel_profile * self.dt) # save as self variables so we can check on the next generate call if we # need a different profile self.start_velocity = start_velocity self.target_velocity = target_velocity if self.verbose: self.log.append( f"{c.blue}Generating a path from {start_position} to " + f"{target_position}{c.endc}") self.log.append( f"{c.blue}max_velocity={self.max_velocity}{c.endc}") self.log.append( f"{c.blue}start_velocity={self.start_velocity} | " + f"target_velocity={self.target_velocity}{c.endc}") # calculate the distance between our current state and the target target_direction = target_position - start_position dist = np.linalg.norm(target_direction) target_norm = target_direction / dist # the default direction of our path shape a = 1 / np.sqrt(3) base_norm = np.array([a, a, a]) # get rotation matrix to rotate our path shape to the target direction R = self.align_vectors(base_norm, target_norm) # get the length travelled along our stretched curve curve_dist_steps = [] warped_xyz = [] for ii, t in enumerate(np.linspace(0, 1, self.n_sample_points)): # ==== Warp our path ==== # - stretch our curve shape to be the same length as target-start # - rotate our curve to align with the direction target-start # - shift our curve to begin at the start state warped_target = ( np.dot(R, (1 / np.sqrt(3)) * self.pos_profile.step(t) * dist) + start_position) warped_xyz.append(warped_target) if t > 0: curve_dist_steps.append( np.linalg.norm(warped_xyz[ii] - warped_xyz[ii - 1])) else: curve_dist_steps.append(0) # get the cumulative distance covered dist_steps = np.cumsum(curve_dist_steps) curve_length = np.sum(curve_dist_steps) # create functions that return our path at a given distance # along that curve self.warped_xyz = np.array(warped_xyz) X = scipy.interpolate.interp1d(dist_steps, self.warped_xyz.T[0], fill_value="extrapolate") Y = scipy.interpolate.interp1d(dist_steps, self.warped_xyz.T[1], fill_value="extrapolate") Z = scipy.interpolate.interp1d(dist_steps, self.warped_xyz.T[2], fill_value="extrapolate") XYZ = [X, Y, Z] # distance is greater than our ramping up and down distance, add a linear # velocity between the ramps to converge to the correct position self.remaining_dist = None if curve_length >= self.starting_dist + self.ending_dist: # calculate the remaining steps where we will be at constant max_v remaining_dist = curve_length - (self.ending_dist + self.starting_dist) constant_speed_steps = int(remaining_dist / self.max_velocity / self.dt) self.stacked_vel_profile = np.hstack(( self.starting_vel_profile, np.ones(constant_speed_steps) * self.max_velocity, self.ending_vel_profile, )) if plot: self.remaining_dist = remaining_dist self.dist = dist else: # scale our profile # TODO to do this properly we should evaluate the integral to get # the t where the sum of the profile is half our travel distance. # This way we maintain the same acceleration profile instead of # maintaining the same number of steps and accelerating more slowly, # which is what we do by scaling the vel profile down scale = curve_length / (self.starting_dist + self.ending_dist) self.stacked_vel_profile = np.hstack( (scale * self.starting_vel_profile, scale * self.ending_vel_profile)) self.position_path = [] # the distance covered over time with respect to our velocity profile path_steps = np.cumsum(self.stacked_vel_profile * self.dt) # step along our curve, with our next path step being the distance # determined by our velocity profile, in the direction of the path curve for ii in range(0, len(self.stacked_vel_profile)): shiftx = XYZ[0](path_steps[ii]) shifty = XYZ[1](path_steps[ii]) shiftz = XYZ[2](path_steps[ii]) shift = np.array([shiftx, shifty, shiftz]) self.position_path.append(shift) self.position_path = np.asarray(self.position_path) # get our 3D vel profile components by differentiating the position path # our velocity profile is 1D, to get the 3 velocity components we can # just differentiate the position path. Since the distance between steps # was determined with our velocity profile, we should still maintain the # desired velocities. Note this may break down with complex, high # frequency paths self.velocity_path = np.asarray( np.gradient(self.position_path, self.dt, axis=0)) # check if we received start and target orientations if isinstance(start_orientation, (list, (np.ndarray, np.generic), tuple)): if isinstance(target_orientation, (list, (np.ndarray, np.generic), tuple)): # Generate the orientation portion of our trajectory. # We will use quaternions and SLERP for filtering orientation path quat0 = transform.quaternion_from_euler( start_orientation[0], start_orientation[1], start_orientation[2], axes=self.axes, ) quat1 = transform.quaternion_from_euler( target_orientation[0], target_orientation[1], target_orientation[2], axes=self.axes, ) self.orientation_path = self.OrientationPlanner.match_position_path( orientation=quat0, target_orientation=quat1, position_path=self.position_path, ) if self.verbose: self.log.append( f"{c.blue}start_orientation={start_orientation} | " + f"target_orientation={target_orientation}{c.endc}") else: raise NotImplementedError( f"{c.red}A target orientation is required to generate path{c.endc}" ) self.orientation_path = np.asarray(self.orientation_path) # TODO should this be included? look at proper derivation here... # https://physics.stackexchange.com/questions/73961/angular-velocity-expressed-via-euler-angles # pylint: disable=C0301 self.ang_velocity_path = np.asarray( np.gradient(self.orientation_path, self.dt, axis=0)) self.path = np.hstack(( np.hstack(( np.hstack((self.position_path, self.velocity_path)), self.orientation_path, )), self.ang_velocity_path, )) else: self.path = np.hstack((self.position_path, self.velocity_path)) if plot: self._plot( start_position=start_position, target_position=target_position, ) # Some parameters that are useful to have access to externally, # used in nengo-control self.n_timesteps = len(self.path) self.n = 0 self.time_to_converge = self.n_timesteps * self.dt self.target_counter += 1 if self.verbose: self.log.append( f"{c.blue}Time to converge: {self.time_to_converge}{c.endc}") self.log.append(f"{c.blue}dt: {self.dt}{c.endc}") self.log.append( f"{c.blue}pos x error: " + f"{self.position_path[-1, 0] - target_position[0]}{c.endc}") self.log.append( f"{c.blue}pos y error: " + f"{self.position_path[-1, 1] - target_position[1]}{c.endc}") self.log.append( f"{c.blue}pos x error: " + f"{self.position_path[-1, 2] - target_position[2]}{c.endc}") self.log.append( f"{c.blue}2norm error at target: " + f"{np.linalg.norm(self.position_path[-1] - target_position[:3])}{c.endc}" # pylint: disable=C0301 ) dash = "".join((["-"] * len(max(self.log, key=len)))) print(f"{c.blue}{dash}{c.endc}") for log in self.log: print(log) print(f"{c.blue}{dash}{c.endc}") err = np.linalg.norm(self.position_path[-1] - target_position) if err >= 0.01: warnings.warn( (f"\n{c.yellow}WARNING: the distance at the end of the " + f"generated path to your desired target position is {err}m." + "If you desire a lower error you can try:" + "\n\t- a path shape with lower frequency terms" + "\n\t- more sample points (set on __init__)" + "\n\t- smaller simulation timestep" + "\n\t- lower maximum velocity and acceleration" + f"\n\t- lower start and end velocities{c.endc}")) return self.path
def generate_path( self, position, target_position, n_timesteps=200, dt=0.001, plot=False, method=3, axes="rxyz", ): """ Parameters ---------- position: numpy.array the current position of the system target_position: numpy.array the task space target position and orientation n_timesteps: int, optional (Default: 200) the number of time steps to reach the target_position dt: float, optional (Default: 0.001) the time step for calculating desired velocities [seconds] plot: boolean, optional (Default: False) plot the path after generating if True method: int, optional (Default: 3) Different ways to compute inverse resolved motion 1. Standard resolved motion 2. Dampened least squares method 3. Nullspace with priority for position, orientation in null space axes: string, optional (Default: 'rxyz') the format of the Euler angles passed in target[3:6] First letter r or s represents 'relative' or 'static' """ path = np.zeros((n_timesteps, position.shape[0] * 2)) ee_track = [] ee_err = [] ea_err = [] # set the largest allowable step in joint position max_dq = self.max_dq * dt # set the largest allowable step in hand (x,y,z) max_dx = self.max_dx * dt # set the largest allowable step in hand (alpha, beta, gamma) max_dr = self.max_dr * dt Qd = np.array( transformations.unit_vector( transformations.quaternion_from_euler( target_position[3], target_position[4], target_position[5], axes="sxyz", ) ) ) q = np.copy(position) for ii in range(n_timesteps): J = self.robot_config.J("EE", q=q) Tx = self.robot_config.Tx("EE", q=q) ee_track.append(Tx) dx = target_position[:3] - Tx Qe = self.robot_config.quaternion("EE", q=q) # Method 4 dr = Qe[0] * Qd[1:] - Qd[0] * Qe[1:] - np.cross(Qd[1:], Qe[1:]) norm_dx = np.linalg.norm(dx, 2) norm_dr = np.linalg.norm(dr, 2) ee_err.append(norm_dx) ea_err.append(norm_dr) # limit max step size in operational space if norm_dx > max_dx: dx = dx / norm_dx * max_dx if norm_dr > max_dr: dr = dr / norm_dr * max_dr Jx = J[:3] pinv_Jx = np.linalg.pinv(Jx) # Different ways to compute inverse resolved motion if method == 1: # Standard resolved motion dq = np.dot(np.linalg.pinv(J), np.hstack([dx, dr])) if method == 2: # Dampened least squares method dq = np.dot( J.T, np.linalg.solve( np.dot(J, J.T) + np.eye(6) * 0.001, np.hstack([dx, dr * 0.3]) ), ) if method == 3: # Primary position IK, control orientation in null space dq = np.dot(pinv_Jx, dx) + np.dot( np.eye(self.robot_config.N_JOINTS) - np.dot(pinv_Jx, Jx), np.dot(np.linalg.pinv(J[3:]), dr), ) # limit max step size in joint space if max(abs(dq)) > max_dq: dq = dq / max(abs(dq)) * max_dq path[ii] = np.hstack([q, dq]) q = q + dq if plot: ee_track = np.array(ee_track) plt.subplot(2, 1, 1) plt.plot(ee_track) plt.gca().set_prop_cycle(None) plt.plot(np.ones((n_timesteps, 3)) * target_position[:3], "--") plt.legend( ["%i" % ii for ii in range(3)] + ["%i_target" % ii for ii in range(3)] ) plt.title("Trajectory positions") plt.subplot(2, 1, 2) plt.plot(ee_err) plt.plot(ea_err) plt.legend(["Position error", "Orientation error"]) plt.title("Trajectory orientations") plt.tight_layout() plt.show() plt.savefig("IK_plot.png") # reset position_path index self.n_timesteps = n_timesteps self.n = 0 self.position_path = path[:, : self.robot_config.N_JOINTS] self.velocity_path = path[:, self.robot_config.N_JOINTS :] return self.position_path, self.velocity_path
Mx = np.linalg.inv(Mx_inv) else: # using the rcond to set singular values < thresh to 0 # singular values < (rcond * max(singular_values)) set to 0 Mx = np.linalg.pinv(Mx_inv, rcond=.005) u_task = np.zeros(6) # [x, y, z, alpha, beta, gamma] # 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
orientation_path, target_position, target_orientation, ) = get_target(robot_config) interface.set_mocap_xyz("target_orientation", target_position) interface.set_mocap_orientation("target_orientation", target_orientation) pos, vel = position_planner.next() orient = orientation_path.next() target = np.hstack([pos, orient]) interface.set_mocap_xyz("path_planner_orientation", target[:3]) interface.set_mocap_orientation( "path_planner_orientation", transformations.quaternion_from_euler(orient[0], orient[1], orient[2], "rxyz"), ) u = ctrlr.generate( q=feedback["q"], dq=feedback["dq"], target=target, ) # add gripper forces u = np.hstack((u, np.zeros(robot_config.N_GRIPPER_JOINTS))) # apply the control signal, step the sim forward interface.send_forces(u) # track data
size=3) path_planner.generate_path( start_position=hand_xyz, target_position=target_position, start_orientation=starting_orientation, target_orientation=target_orientation, max_velocity=2, ) interface.set_mocap_xyz("target_orientation", target_position) interface.set_mocap_orientation( "target_orientation", transformations.quaternion_from_euler( target_orientation[0], target_orientation[1], target_orientation[2], "rxyz", ), ) next_target = path_planner.next() pos = next_target[:3] vel = next_target[3:6] orient = next_target[6:9] target = np.hstack([pos, orient]) interface.set_mocap_xyz("path_planner_orientation", target[:3]) interface.set_mocap_orientation( "path_planner_orientation", transformations.quaternion_from_euler(orient[0], orient[1], orient[2], "rxyz"),