def send_forces(self, u): """ Apply the specified torque to the robot joints Apply the specified torque to the robot joints, move the simulation one time step forward, and update the position of the hand object. Parameters ---------- u : np.array the torques to apply to the robot """ # invert because CoppeliaSim torque directions are opposite of expected u *= -1 for ii, joint_handle in enumerate(self.joint_handles): # get the current joint torque _, torque = sim.simxGetJointForce(self.clientID, joint_handle, self.blocking) if _ != 0: raise Exception( "Error retrieving joint torque, " + "return code ", _) # if force has changed signs, # we need to change the target velocity sign if np.sign(torque) * np.sign(u[ii]) <= 0: self.joint_target_velocities[ ii] = self.joint_target_velocities[ii] * -1 _ = sim.simxSetJointTargetVelocity( self.clientID, joint_handle, self.joint_target_velocities[ii], self.set_opmode, ) # and now modulate the force _ = sim.simxSetJointForce( self.clientID, joint_handle, abs(u[ii]), # force to apply self.set_opmode, ) # Update position of hand object hand_xyz = self.robot_config.Tx(name="EE", q=self.q) self.set_xyz("hand", hand_xyz) # Update orientation of hand object angles = transformations.euler_from_matrix(self.robot_config.R( "EE", q=self.q), axes="rxyz") self.set_orientation("hand", angles) # move simulation ahead one time step sim.simxSynchronousTrigger(self.clientID) self.count += self.dt
def on_keypress(self, key): if key == pygame.K_LEFT: self.theta += np.pi / 10 if key == pygame.K_RIGHT: self.theta -= np.pi / 10 print('theta: ', self.theta) # set the target orientation to be the initial EE # orientation rotated by theta R_theta = np.array([ [np.cos(interface.theta), -np.sin(interface.theta), 0], [np.sin(interface.theta), np.cos(interface.theta), 0], [0, 0, 1]]) R_target = np.dot(R_theta, R) self.target_angles = transformations.euler_from_matrix( R_target, axes='sxyz')
u = ctrlr.generate( q=feedback['q'], dq=feedback['dq'], target=target, #target_vel=np.hstack([vel, np.zeros(3)]) ) # apply the control signal, step the sim forward interface.send_forces(u) # track data ee_track.append(np.copy(hand_xyz)) ee_angles_track.append( transformations.euler_from_matrix(robot_config.R( 'EE', feedback['q']), axes='rxyz')) target_track.append(np.copy(target[:3])) target_angles_track.append(np.copy(target[3:])) count += 1 finally: # stop and reset the simulation interface.disconnect() print('Simulation terminated...') ee_track = np.array(ee_track).T ee_angles_track = np.array(ee_angles_track).T target_track = np.array(target_track).T target_angles_track = np.array(target_angles_track).T
count = 0 while 1: if interface.viewer.exit: glfw.destroy_window(interface.viewer.window) break if count % n_timesteps == 0: feedback = interface.get_feedback() target_xyz = np.array([ np.random.random() * 0.5 - 0.25, np.random.random() * 0.5 - 0.25, np.random.random() * 0.5 + 0.5, ]) R = robot_config.R("EE", q=feedback["q"]) target_orientation = transformations.euler_from_matrix(R, "sxyz") # update the position of the target interface.set_mocap_xyz("target", target_xyz) # can use 3 different methods to calculate inverse kinematics # see inverse_kinematics.py file for details path_planner.generate_path( position=feedback["q"], target_position=np.hstack([target_xyz, target_orientation]), method=3, dt=0.005, n_timesteps=n_timesteps, plot=False, ) # returns desired [position, velocity]
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 ee_track.append(np.copy(hand_xyz)) ee_angles_track.append( transformations.euler_from_matrix(robot_config.R( "EE", feedback["q"]), axes="rxyz")) target_track.append(np.copy(target[:3])) target_angles_track.append(np.copy(target[3:])) count += 1 finally: # stop and reset the simulation interface.disconnect() print("Simulation terminated...") ee_track = np.array(ee_track) ee_angles_track = np.array(ee_angles_track) target_track = np.array(target_track) target_angles_track = np.array(target_angles_track)
if (count % 200) == 0: # change target once every simulated second if index >= len(orientations): break interface.set_orientation('target', orientations[index]) interface.set_xyz('target', positions[index]) index += 1 target = np.hstack([ interface.get_xyz('target'), interface.get_orientation('target')]) # set the block to be the same orientation as end-effector R_e = robot_config.R('EE', feedback['q']) EA_e = transformations.euler_from_matrix(R_e, axes='rxyz') interface.set_orientation('object', EA_e) # calculate the Jacobian for the end effectora # and isolate relevate dimensions J = robot_config.J('EE', q=feedback['q'])[ctrlr_dof] # calculate the inertia matrix in task space M = robot_config.M(q=feedback['q']) # calculate the inertia matrix in task space M_inv = np.linalg.inv(M) Mx_inv = np.dot(J, np.dot(M_inv, J.T)) if np.linalg.det(Mx_inv) != 0: # do the linalg inverse if matrix is non-singular # because it's faster and more accurate
interface.get_mocap_xyz('target_orientation'), transformations.euler_from_quaternion( interface.get_mocap_orientation('target_orientation'), '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 ee_track.append(np.copy(hand_xyz)) ee_angles_track.append(transformations.euler_from_matrix( robot_config.R('EE', feedback['q']), axes='rxyz')) target_track.append(np.copy(target[:3])) target_angles_track.append(np.copy(target[3:])) count += 1 finally: # stop and reset the simulation interface.disconnect() print('Simulation terminated...') ee_track = np.array(ee_track) ee_angles_track = np.array(ee_angles_track) target_track = np.array(target_track) target_angles_track = np.array(target_angles_track)
hand_xyz = robot_config.Tx('EE', feedback['q']) if (count % 200) == 0: # change target once every simulated second if index >= len(orientations): break interface.set_orientation('target', orientations[index]) index += 1 target = np.hstack([ interface.get_xyz('target'), interface.get_orientation('target')]) # set the block to be the same orientation as end-effector R_e = robot_config.R('EE', feedback['q']) EA_e = transformations.euler_from_matrix(R_e, axes='rxyz') interface.set_orientation('object', EA_e) # calculate the Jacobian for the end effectora # and isolate relevate dimensions J = robot_config.J('EE', q=feedback['q'])[ctrlr_dof] # calculate the inertia matrix in task space M = robot_config.M(q=feedback['q']) # calculate the inertia matrix in task space M_inv = np.linalg.inv(M) Mx_inv = np.dot(J, np.dot(M_inv, J.T)) if np.linalg.det(Mx_inv) != 0: # do the linalg inverse if matrix is non-singular # because it's faster and more accurate