def generate_path(self, orientation, target_orientation, plot=False): """ Generates a linear trajectory to the target Accepts orientations as quaternions and returns an array of orientations from orientation to target orientation, based on the timesteps defined in __init__. Orientations are returns as euler angles to match the format of the OSC class NOTE: no velocity trajectory is calculated at the moment Parameters ---------- orientation: list of 4 floats the starting orientation as a quaternion target_orientation: list of 4 floats the target orientation as a quaternion """ # stores the target Euler angles of the trajectory self.orientation = np.zeros((self.n_timesteps, 3)) self.target_angles = transformations.euler_from_quaternion( target_orientation, axes='rxyz') for ii in range(self.n_timesteps): quat = self._step(orientation=orientation, target_orientation=target_orientation) self.orientation[ii] = transformations.euler_from_quaternion( quat, axes='rxyz') self.n = 0 return self.orientation
def generate_path(self, orientation, target_orientation, plot=False): """ Generates a linear trajectory to the target Accepts orientations as quaternions and returns an array of orientations from orientation to target orientation, based on the timesteps defined in __init__. Orientations are returns as euler angles to match the format of the OSC class NOTE: no velocity trajectory is calculated at the moment Parameters ---------- orientation: list of 4 floats the starting orientation as a quaternion target_orientation: list of 4 floats the target orientation as a quaternion """ if len(orientation) == 3: raise ValueError( "\n----------------------------------------------\n" + "A quaternion is required as input for the orientation " + "path planner. To convert your " + "Euler angles into a quaternion run...\n\n" + "from abr_control.utils import transformations\n" + "quaternion = transformation.quaternion_from_euler(a, b, g)\n" + "----------------------------------------------") # stores the target Euler angles of the trajectory self.orientation_path = np.zeros((self.n_timesteps, 3)) self.target_angles = transformations.euler_from_quaternion( target_orientation, axes="rxyz") for ii in range(self.n_timesteps): quat = self._step(orientation=orientation, target_orientation=target_orientation) self.orientation_path[ii] = transformations.euler_from_quaternion( quat, axes="rxyz") self.n = 0 if plot: self._plot() return self.orientation_path
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)
interface.set_mocap_xyz(name="target", xyz=target_xyz) # set the status of the top right text for adaptation interface.viewer.adapt = True count = 0.0 while 1: if interface.viewer.exit: glfw.destroy_window(interface.viewer.window) break # get joint angle and velocity feedback feedback = interface.get_feedback() target = np.hstack([ interface.get_xyz("target"), transformations.euler_from_quaternion( interface.get_orientation("target"), "rxyz"), ]) # calculate the control signal u = ctrlr.generate( q=feedback["q"], dq=feedback["dq"], target=target, ) u_adapt = np.zeros(robot_config.N_JOINTS) u_adapt[:5] = adapt.generate( input_signal=np.hstack((feedback["q"][:5], feedback["dq"][:5])), training_signal=np.array(ctrlr.training_signal[:5]), ) u += u_adapt
n_timesteps=n_timesteps) orientation_planner = traj_planner.generate_orientation_path( orientation=starting_orientation, target_orientation=target_orientation) # set up lists for tracking data ee_track = [] ee_angles_track = [] target_track = [] target_angles_track = [] try: count = 0 interface.set_xyz('target', target_position) interface.set_orientation( 'target', transformations.euler_from_quaternion(target_orientation, axes='rxyz')) print('\nSimulation starting...\n') while 1: # get arm feedback feedback = interface.get_feedback() hand_xyz = robot_config.Tx('EE', feedback['q']) pos, vel = traj_planner.next()[:3] orient = orientation_planner.next() target = np.hstack([pos, orient]) u = ctrlr.generate( q=feedback['q'], dq=feedback['dq'], target=target,
if interface.viewer.exit: glfw.destroy_window(interface.viewer.window) break # get arm feedback feedback = interface.get_feedback() hand_xyz = robot_config.Tx('EE', feedback['q']) # set our target to our ee_xyz since we are only focussing on orinetation interface.set_mocap_xyz('target_orientation', hand_xyz) interface.set_mocap_orientation('target_orientation', rand_orient) target = np.hstack([ interface.get_mocap_xyz('target_orientation'), transformations.euler_from_quaternion( interface.get_mocap_orientation('target_orientation'), 'rxyz') ]) rc_matrix = robot_config.R('EE', feedback['q']) rc_angles = transformations.euler_from_matrix(rc_matrix, axes='rxyz') u = ctrlr.generate( q=feedback['q'], dq=feedback['dq'], target=target, ) # apply the control signal, step the sim forward interface.send_forces(u) # track data
# traj_planner = path_planners.BellShaped( # error_scale=1, n_timesteps=n_timesteps) traj_planner = path_planners.SecondOrderFilter(n_timesteps=n_timesteps, dt=0.004) orientation_planner = path_planners.Orientation() feedback = interface.get_feedback() hand_xyz = robot_config.Tx('EE', feedback['q']) starting_orientation = robot_config.quaternion('EE', feedback['q']) target_orientation = np.random.random(3) target_orientation /= np.linalg.norm(target_orientation) # convert our orientation to a quaternion target_orientation = [0] + list(target_orientation) print(target_orientation) target_orientation_euler = transformations.euler_from_quaternion( target_orientation) target_position = np.array([-0.4, -0.3, 0.5]) #np.random.random(3) traj_planner.generate_path(position=hand_xyz, target_position=target_position, velocity=np.zeros(3)) orientation_planner.match_position_path( orientation=starting_orientation, target_orientation=target_orientation, position_path=traj_planner.position_path) # set up lists for tracking data ee_track = [] ee_angles_track = [] target_track = [] target_angles_track = []
def generate_path(self, orientation, target_orientation, dr=None, plot=False): """Generates a linear trajectory to the target Accepts orientations as quaternions and returns an array of orientations from orientation to target orientation, based on the timesteps defined in __init__. Orientations are returns as euler angles to match the format of the OSC class NOTE: no velocity trajectory is calculated at the moment Parameters ---------- orientation: list of 4 floats the starting orientation as a quaternion target_orientation: list of 4 floats the target orientation as a quaternion dr: float, Optional (Default: None) if not None the path to target is broken up into n_timesteps segments. Otherwise the number of timesteps are determined based on the set step size in radians. """ if len(orientation) == 3: raise ValueError( "\n----------------------------------------------\n" + "A quaternion is required as input for the orientation " + "path planner. To convert your " + "Euler angles into a quaternion run...\n\n" + "from abr_control.utils import transformations\n" + "quaternion = transformation.quaternion_from_euler(a, b, g)\n" + "----------------------------------------------") self.target_angles = transformations.euler_from_quaternion( target_orientation, axes=self.axes) if dr is not None: # angle between two quaternions # "https://www.researchgate.net/post" # + "/How_do_I_calculate_the_smallest_angle_between_two_quaternions" # answer by luiz alberto radavelli angle_diff = 2 * np.arccos( np.dot(target_orientation, orientation) / (np.linalg.norm(orientation) * np.linalg.norm(target_orientation))) if angle_diff > np.pi: min_angle_diff = 2 * np.pi - angle_diff else: min_angle_diff = angle_diff self.n_timesteps = int(min_angle_diff / dr) print("%i steps to cover %f rad in %f sized steps" % (self.n_timesteps, angle_diff, dr)) self.timesteps = np.linspace(0, 1, self.n_timesteps) # stores the target Euler angles of the trajectory self.orientation_path = np.zeros((self.n_timesteps, 3)) for ii in range(self.n_timesteps): quat = self._step(orientation=orientation, target_orientation=target_orientation) self.orientation_path[ii] = transformations.euler_from_quaternion( quat, axes=self.axes) if self.n_timesteps == 0: print("with the set step size, we reach the target in 1 step") self.orientation_path = np.array([ transformations.euler_from_quaternion(target_orientation, axes=self.axes) ]) self.n = 0 if plot: self._plot() return self.orientation_path
print("\nSimulation starting...\n") while 1: if interface.viewer.exit: glfw.destroy_window(interface.viewer.window) break # get arm feedback feedback = interface.get_feedback() hand_xyz = robot_config.Tx("EE", feedback["q"]) if first_pass or count == path_planner.n_timesteps + 500: count = 0 first_pass = False # pregenerate our path and orientation planners q = robot_config.quaternion("EE", feedback["q"]) starting_orientation = transformations.euler_from_quaternion( q, axes="rxyz") mag = 0.6 target_position = np.random.random(3) * 0.5 target_position = target_position / np.linalg.norm( target_position) * mag target_orientation = np.random.uniform(low=-np.pi, high=np.pi, size=3) path_planner.generate_path( start_position=hand_xyz, target_position=target_position, start_orientation=starting_orientation, target_orientation=target_orientation,