def move_to_pose(self, pose, Tf, N, inter_type, frame='end'): while self.joint_position_received == False: rospy.loginfo('Waiting for joint angles...') rospy.sleep(1) # Cartisian or joint interplation if inter_type == 'c': x_start = self.forward_kinematics(self.joint_position, frame) print('xstart', x_start) x_end = pose print('xend', x_end) x_traj = mr.CartesianTrajectory(x_start, x_end, Tf, N, 5) #print(x_traj) traj = [] for i in range(len(x_traj)): if i == 0: theta0 = self.joint_position else: theta0 = traj[i - 1] #print(x_traj[i]) #print(theta0) theta, _ = self.inverse_kinematics(x_traj[i], theta0, frame) #print(theta) traj.append(theta) elif inter_type == 'j': theta_d = self.inverse_kinematics(pose, self.joint_position, frame) traj = mr.JointTrajectory(self.joint_position, theta_d, Tf, N, 5) dt = Tf / (N - 1.0) self.move_joint_traj(traj, dt)
def move_to_joint_angle(self, angles, T, N): while self.joint_position_received == False: rospy.loginfo('Waiting for joint angles...') rospy.sleep(1) traj = mr.JointTrajectory(self.joint_position, angles, T, N, 5) dt = T / (N - 1.0) self.move_joint_traj(traj, dt)
def move_to_pose(self, pose, jaw_angle, Tf, N, inter_type, frame='jaw'): while self.joint_angle_received == False: self.loginfo_wait_joint_angle() rospy.sleep(1) traj = [] # self.set_last_tar_joint_angles2cur() # Cartisian or joint interplation if inter_type == 'c': x_start = self.get_current_pos() if self.last_desired_pose is not None: x_start = self.last_desired_pose print('xstart', x_start) x_end = pose print('xend', x_end) x_traj = mr.CartesianTrajectory(x_start, x_end, Tf, N, 5) jaw_traj = mr.JointTrajectory([self.jaw_angle], [jaw_angle], Tf, N, 5) #print(x_traj) for i in range(len(x_traj)): if i == 0: theta0 = self.joint_angle[:-1] else: theta0 = traj[i - 1] #print(x_traj[i]) #print(theta0) theta_d = self.inverse_kinematics_rcm(x_traj[i]) motor_angle_d = self.joint_angle2motor_angle( theta_d, jaw_traj[i]) #print(theta) traj.append(motor_angle_d) elif inter_type == 'j': theta_d = self.inverse_kinematics_rcm(pose) print("theta_d: {}".format(theta_d)) motor_angle_d = self.joint_angle2motor_angle(theta_d, jaw_angle) print("motor_d: {}".format(motor_angle_d)) print("tar motor angle: {}".format(motor_angle_d)) traj = mr.JointTrajectory(self.motor_angle, motor_angle_d, Tf, N, 5) dt = Tf / (N - 1.0) self.move_joint_traj(traj, dt) self.last_desired_pose = pose
def move_to_joint_angle(self, joint_angles, jaw_angle, T, N): while self.joint_angle_received == False: self.loginfo_wait_joint_angle() rospy.sleep(1) self.tar_jaw_angle = jaw_angle motor_angle_d = self.joint_angle2motor_angle(joint_angles, jaw_angle) # print('current_motor_angles {}'.format(self.motor_angle)) # print('desired_motor_angles {}'.format(motor_angle_d)) traj = mr.JointTrajectory(self.motor_angle, motor_angle_d, T, N, 5) dt = T / (N - 1.0) self.move_joint_traj(traj, dt) self.last_desired_pose = None
def _generate_joint_trajectory(self): """ Computes a straight-line trajectory in joint space """ self.traj = mr.JointTrajectory(self.jointstart, self.jointend, self.Tf, self.N, self.method) self.thetamat = np.array(self.traj).copy() self.dthetamat = np.zeros((self.N, self.arm_dof)) self.ddthetamat = np.zeros((self.N, self.arm_dof)) for i in range(np.array(self.traj).shape[0] - 1): self.dthetamat[i + 1, :] = (self.thetamat[i + 1, :] - self.thetamat[i, :]) / self.dt self.ddthetamat[i + 1, :] \ = (self.dthetamat[i + 1, :] - self.dthetamat[i, :]) / self.dt
def start(text): global thetastart global thetaend global trail thetaend = findEndThetas() traj = mr.JointTrajectory(thetastart, thetaend, Tf, N, method) jointPositionMat = np.array( [ForwardK.updateMatrices(traj[i])[0] for i in range(N)]) for i in range(N): drawEverything(jointPositionMat[i]) print(jointPositionMat[i]) print("pos {}".format(i + 1)) plt.pause(0.005) # reset everything TODO fix errors this might be causing thetastart = thetaend thetaend = np.zeros(6) trail = np.empty([1, 3])
def test_inverse_dynamics_trajectory(self): # Create a trajectory to follow using functions from Chapter 9 thetastart = np.array([0, 0, 0]) thetaend = np.array([np.pi / 2, np.pi / 2, np.pi / 2]) Tf = 3 N = 1000 method = 5 traj = mr.JointTrajectory(thetastart, thetaend, Tf, N, method) thetamat = np.array(traj).copy() dthetamat = np.zeros((1000, 3)) ddthetamat = np.zeros((1000, 3)) dt = Tf / (N - 1.0) for i in range(np.array(traj).shape[0] - 1): dthetamat[i + 1, :] = (thetamat[i + 1, :] - thetamat[i, :]) / dt ddthetamat[i + 1, :] \ = (dthetamat[i + 1, :] - dthetamat[i, :]) / dt # Initialize robot description (Example with 3 links) g = np.array([0, 0, -9.8]) Ftipmat = np.ones((N, 6)) M01 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0.089159], [0, 0, 0, 1]]) M12 = np.array([[0, 0, 1, 0.28], [0, 1, 0, 0.13585], [-1, 0, 0, 0], [0, 0, 0, 1]]) M23 = np.array([[1, 0, 0, 0], [0, 1, 0, -0.1197], [0, 0, 1, 0.395], [0, 0, 0, 1]]) M34 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0.14225], [0, 0, 0, 1]]) G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) Glist = np.array([G1, G2, G3]) Mlist = np.array([M01, M12, M23, M34]) Slist = np.array([[1, 0, 1, 0, 1, 0], [0, 1, 0, -0.089, 0, 0], [0, 1, 0, -0.089, 0, 0.425]]).T correct_taumat \ = mr.InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, Ftipmat, Mlist, Glist, Slist) taumat = util.InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, Ftipmat, Mlist, Glist, Slist) self.assertTrue(True, matrix_equal(correct_taumat, taumat))
def __on_to_position_pid(self, currentphi_func, targetphi, timestep, method=MOVEMENT_TIME_SCALING_METHOD_QUINTIC, speedpercent=5, stop_action=None, dryrun=False): ''' Threaded implementation of PID controler for motor. param:currentphi_func:desc:current angle in degrees (can't obtain it from motor as it's unsafe to read motor position) param:currentphi_func:type:float param:targetphi:desc:angle in degrees to move param:targetphi:type:float param:speedpercent:desc:value between ]0, 100]. 100 = 100% of max speed, 0 = 0% param:speedpercent:float param:stop_action:desc:see values in LargeMotor(Motor) param:stop_action:type:str param:dryrun:desc:if True, won't move param:dryrun:type:bool returns: no value is returned ''' LOGGER_MOTOR_OTPPID_INSTANCE = LOGGER_MOTOR_OTPPID.format(self.address) wiggle = 1 # tolerated degrees for motor final position # 0. Check for urgent stop if self.urgentstop: log.critical(LOGGER_MOTOR_OTPPID_INSTANCE, "Initiating urgent stop of base motors.") self.off() log.critical(LOGGER_MOTOR_OTPPID_INSTANCE, "Urgent stop of base motors completed.") return # 1. Estimate time to target: theta = currentphi_func eta = abs((targetphi - degrees(theta())) / self.max_dps / (speedpercent / 100) * 2) # /2 accounts for ramp up and down - it's approximate # 2. find trajectory in joint/wheel angle space: log.info(LOGGER_MOTOR_OTPPID_INSTANCE, 'Figuring out trajectory in joint space') trajectory = mr.JointTrajectory(thetastart=[degrees(theta())], thetaend=[targetphi], Tf=eta, N=max(2, int(eta / timestep)), method=method) # 3. estimate speed for next segment idx = 0 adj_step_count = 0 while not (targetphi - wiggle <= degrees(theta()) and degrees(theta()) <= targetphi + wiggle): tic = time.time() theta_desired = trajectory[min(idx, len(trajectory) - 1)] theta_desired_next = trajectory[min(idx + 1, len(trajectory) - 1)] if self.urgentstop: log.critical(LOGGER_MOTOR_OTPPID_INSTANCE, 'Stopping motion. Urgent stop called.') break # Get current error theta_err = theta_desired - degrees(theta()) # Desired Speed (based on ideal trajectory) theta_dot_desired = ( theta_desired_next - theta_desired) / timestep # will be 0 after initial trajectory # Actual PID controler on speed theta_dot = int(theta_dot_desired + self.kp * theta_err + self.ki * theta_err * timestep) # Clip the speed if outside of boundaries theta_dot = np.clip(theta_dot, -self.max_speed, self.max_speed) # Do not run unless it's not a dryrun and speed > 0 if not dryrun: if theta_dot != 0: log.debug(LOGGER_MOTOR_OTPPID_INSTANCE, f'Setting speed to {theta_dot} for motor') self.on(SpeedDPS(theta_dot), brake=True, block=False) else: log.debug( LOGGER_MOTOR_OTPPID_INSTANCE, "Step {0} - Not moving motor {1} : set point = {2}". format(idx, self.kwargs['address'], theta_dot)) else: log.debug( LOGGER_MOTOR_OTPPID_INSTANCE, "Step {0} - Not moving motor {1} : dry run".format( idx, self.kwargs['address'])) stepDuration = time.time() - tic if idx % 5 == 0: if adj_step_count > 0: log.debug( LOGGER_MOTOR_OTPPID_INSTANCE, "Step {0} - Duration of loop = {1:.2f}. Sleeping {2:.2f}s \ until next step. TimeStep = {3:.2f}. Position/Target = {4:.2f}/{5:.2f}" .format(idx + 1, stepDuration, timestep - stepDuration, timestep, degrees(theta()), targetphi)) else: log.debug( LOGGER_MOTOR_OTPPID_INSTANCE, "Fine tuning step {0} - Duration of loop = {1:.2f}. Sleeping {2:.2f}s \ until next step. TimeStep = {3:.2f}. Position/Target = {4:.2f}/{5:.2f}" .format(adj_step_count, stepDuration, timestep - stepDuration, timestep, degrees(theta()), targetphi)) time.sleep(max(0, timestep - stepDuration)) idx += 1 if idx > len(trajectory): adj_step_count += 1 self.stop(stop_action=stop_action) return
def get_traj_joint(theta_s, theta_e): return mr.JointTrajectory(theta_s, theta_e, 5, 3, 3)
Xend[0][3] += 0.1 Xend[1][3] += 0.1 Tf = 3 N = 500 method = 5 traj = mr.CartesianTrajectory(Xstart, Xend, Tf, N, method) print(traj) ''' thetastart = [1, 0, 0, 1, 1, 0.2, 0, 1] thetaend = [1.5, 0.5, 0.6, 1.1, 2, 2, 0.9, 1] Tf = 4.0 N = 100 method = 5 traj = mr.JointTrajectory(thetastart, thetaend, Tf, N, method) #print(traj) #traj = np.array(traj) x = np.linspace(0, Tf, N) d_t = Tf / (N - 1) positions = [] positions_sum = [] velocities = [] accelerations = [] for i in range(len(traj)): p = [0] * 6 v = [0] * 6 last_v = [0] * 6 a = [0] * 6 if i != len(traj) - 1 and i != 0: