def TrajectoryGenerator(Tse_init, Tsc_init, Tsc_final, Tce_grasp, Tce_stand, k):
"""
k: The number of trajectory reference configurations per 0.01 seconds
Ex: k=10, t=0.01; freq_controller = k/t = 1000Hz
Tse_init: The initial configuration of the end-effector for the reference trajectory
Tsc_init: The cube's initial configuration
Tsc_final: The cube's desired final configuration
Tce_grasp: The end-effector's configuration relative to the cube when it is grasping the cube
Tce_stand: The end-effector's standoff configuration above the cube, before and after grasping, relative to the cube
"""
t = 2
N = t*k/0.01
# Move end-effector to cube
Tse_is = np.dot(Tsc_init, Tce_stand)
traj = mr.CartesianTrajectory(Tse_init, Tse_is, t, N, 5)
for i in range(len(traj)):
temp = shuffle(traj[i], 0)
ref_traj.append(temp)
# Drop end-effector to cube height
Tse_ig = np.dot(Tsc_init, Tce_grasp)
traj = mr.CartesianTrajectory(Tse_is, Tse_ig, t, N, 5)
for i in range(len(traj)):
temp = shuffle(traj[i], 0)
ref_traj.append(temp)
# Close gripper
close_gripper()
# Raise end-effector back up
traj = mr.CartesianTrajectory(Tse_ig, Tse_is, t, N, 5)
for i in range(len(traj)):
temp = shuffle(traj[i], 1)
ref_traj.append(temp)
# Move to final position
t = 4
N = t*k/0.01
Tse_fs = np.dot(Tsc_final, Tce_stand)
traj = mr.CartesianTrajectory(Tse_is, Tse_fs, t, N, 5)
for i in range(len(traj)):
temp = shuffle(traj[i], 1)
ref_traj.append(temp)
# Drop end-effector to final cube height
t = 2
N = t*k/0.01
Tse_fg = np.dot(Tsc_final, Tce_grasp)
traj = mr.CartesianTrajectory(Tse_fs, Tse_fg, t, N, 5)
for i in range(len(traj)):
temp = shuffle(traj[i], 1)
ref_traj.append(temp)
# Open gripper
open_gripper()
print("Trajectory Generated")
def path_planning(self):
""" Generate desriable waypoints for achieving target pose.
- Example
dt = 0.01
#Create a trajectory to follow
thetaend = [pi / 2, pi, 1.5 * pi]
Tf = 1
N = Tf / dt
method = 5
dt = Tf / (N - 1.0)
"""
dt = self.sample_time
current_pose = self.limb.endpoint_pose()
X_current = self._get_tf_matrix(current_pose)
X_goal = self.original_goal
# X_goal[2][3] += 0.05
rospy.logwarn('=============== Current pose ===============')
print(X_current)
rospy.logwarn('=============== Goal goal ===============')
print(X_goal)
Tf = self.plan_time
# Tf = np.random.uniform(self.rand_plan_min, self.rand_plan_max)
N = int(Tf / dt) # ex: plantime = 7, dt = 0.01 -> N = 700
self.num_wp = N
self.traj_list = r.CartesianTrajectory(X_current,
X_goal,
Tf=Tf,
N=N,
method=CUBIC)
self.set_init_frame(
) # save the robot's pose frame at start of the trajectory.
self.isPathPlanned = True
def Generate_segment_trajectory(self, T_start, T_end, time_scale,
traj_type):
'''
Generate segment of trajectory of the end effector
Input :
T_start : Starting configuration
T_end : Goal configuration
time_scale : Time scaling of this segment
traj_type : Screw or Cartesian type
Return:
trajs : Trajectory of the segment in [np.array([Tse..]), np.array([next_Tse])]
'''
# Duration and number of configuration of the trajectory is computed based on the maximum linear and angular velocity
T_dist = np.dot(mr.TransInv(T_start), T_end)
dist = math.hypot(T_dist[0, -1], T_dist[1, -1])
ang = max(self.euler_angles_from_rotation_matrix(T_dist[:-1, :-1]))
duration = max(dist / self.max_lin_vel,
ang / self.max_ang_vel) # Duration from rest to rest
no_conf = int(
(duration * self.k) /
0.01) # Number of configuration between the start and goal
if (traj_type == "Screw"):
trajs = mr.ScrewTrajectory(T_start, T_end, duration, no_conf,
time_scale)
elif (traj_type == "Cartesian"):
trajs = mr.CartesianTrajectory(T_start, T_end, duration, no_conf,
time_scale)
return trajs
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 createSegment1(T_se_initial, T_sc_initial, T_ce_standoff, k):
# Need to compute the desired orientation R
T_se_standoff = np.matmul(T_sc_initial.copy(), T_ce_standoff.copy())
totalSeconds = 100
N = float(totalSeconds) / float(k)
X_start = T_se_initial.copy()
X_end = T_se_standoff.copy()
X_end = X_end.astype(float)
# The total time in seconds
Tf = totalSeconds
# Method describes cubic or quintic scaling
method = 5
path_states = mr.CartesianTrajectory(X_start, X_end, Tf, N, method)
# Take the 3-D array and put it into a 2-D form
path_states = process_array(path_states, 0)
return path_states
def TrajectoryGenerator(Tse_i, Tsc_i, Tsc_f, Tce_g, Tce_s,k):
#inital to standoff position
Tse_s = np.dot(Tsc_i, Tce_s)
N = 2*k/0.01
traj = mr.CartesianTrajectory(Tse_i, Tse_s, 2, N, 5)
for i in range(int(N)):
final.append([traj[i][0][0], traj[i][0][1], traj[i][0][2], traj[i][1][0], traj[i][1][1], traj[i][1][2], traj[i][2][0], traj[i][2][1], traj[i][2][2], traj[i][0][3], traj[i][1][3], traj[i][2][3], 0])
#standoff to grasp
Tse_g = np.dot(Tsc_i, Tce_g)
N = 2*k/0.01
traj = mr.CartesianTrajectory(Tse_s, Tse_g, 2, N, 5)
for i in range(int(N)):
final.append([traj[i][0][0], traj[i][0][1], traj[i][0][2], traj[i][1][0], traj[i][1][1], traj[i][1][2], traj[i][2][0], traj[i][2][1], traj[i][2][2], traj[i][0][3], traj[i][1][3], traj[i][2][3], 0])
#closing the grasp
List = final[-1]
closed = []
for i in range(len(List)-1):
closed.append(List[i])
closed.append(1)
for i in range(100):
final.append(closed)
#grasp to standoff
N = 2*k/0.01
traj = mr.CartesianTrajectory(Tse_g, Tse_s, 2, N, 5)
for i in range(int(N)):
final.append([traj[i][0][0], traj[i][0][1], traj[i][0][2], traj[i][1][0], traj[i][1][1], traj[i][1][2], traj[i][2][0], traj[i][2][1], traj[i][2][2], traj[i][0][3], traj[i][1][3], traj[i][2][3], 1])
#standoff to final stand off
N = 5*k/0.01
Tse_fs = np.dot(Tsc_f, Tce_s)
traj = mr.CartesianTrajectory(Tse_s, Tse_fs, 3, N, 5)
for i in range(int(N)):
final.append([traj[i][0][0], traj[i][0][1], traj[i][0][2], traj[i][1][0], traj[i][1][1], traj[i][1][2], traj[i][2][0], traj[i][2][1], traj[i][2][2], traj[i][0][3], traj[i][1][3], traj[i][2][3], 1])
#final standoff to final grasp
N = 2*k/0.01
Tse_fg = np.dot(Tsc_f, Tce_g)
traj = mr.CartesianTrajectory(Tse_fs, Tse_fg, 2, N, 5)
for i in range(int(N)):
final.append([traj[i][0][0], traj[i][0][1], traj[i][0][2], traj[i][1][0], traj[i][1][1], traj[i][1][2], traj[i][2][0], traj[i][2][1], traj[i][2][2], traj[i][0][3], traj[i][1][3], traj[i][2][3], 1])
#opening the grasp
List = final[-1]
opened = []
for i in range(len(List)-1):
opened.append(List[i])
opened.append(0)
for i in range(100):
final.append(opened)
#final grasp to final standoff
N = 2*k/0.01
traj = mr.CartesianTrajectory(Tse_fg, Tse_fs, 2, N, 5)
for i in range(int(N)):
final.append([traj[i][0][0], traj[i][0][1], traj[i][0][2], traj[i][1][0], traj[i][1][1], traj[i][1][2], traj[i][2][0], traj[i][2][1], traj[i][2][2], traj[i][0][3], traj[i][1][3], traj[i][2][3], 0])
def servo_vel(self, pose, time=4.0, steps=400, num_wp=5):
"""Servo the robot to the goal
TODO: merge check_path_stop method
"""
current_pose = self._limb.endpoint_pose()
X_current = self._get_tf_matrix(current_pose)
X_goal = self._get_tf_matrix(pose)
traj = mr.CartesianTrajectory(X_current, X_goal, Tf=5, N=num_wp, method=3)
for idx, X in enumerate(traj):
_pose = self._get_msg(X)
self.ikVelPub.publish(_pose)
self._check_traj(_pose, idx)
def TrajectoryGenerator(Tse_i,Tsc_i,Tsc_f,Tce_g,Tce_s,k): """ Tse_i: The initial configuration of the end-effector in the reference trajectory == Tse_initial. Tsc_i: The cube's initial configuration == Tsc_initial Tsc_f: The cube's desired final configuration == Tsc_final Tce_g: The end-effector's configuration relative to the cube when it is grasping the cube == Tce_grasp Tce_s: The end-effector's standoff configuration above the cube, before and after grasping, relative to the cube == Tce_standoff k: The number of trajectory reference configurations per 0.01 seconds """ Tse_si = np.dot(Tsc_i,Tce_s) # The end-effector's standoff configuration above the initial cube relative to the world frame N = 2*k/0.01 traj1 = mr.CartesianTrajectory(Tse_i,Tse_si,2,N,5) # Move from initial position to the initial standoff position for i in range(int(N)): param.append([traj1[i][0][0],traj1[i][0][1],traj1[i][0][2],traj1[i][1][0],traj1[i][1][1],traj1[i][1][2],\ traj1[i][2][0],traj1[i][2][1],traj1[i][2][2],traj1[i][0][3],traj1[i][1][3],traj1[i][2][3],0])
def Trajectory(Ti,Tf,iterations):
thetalist0 = BestTheta0(Ti[0][3],Ti[1][3],Ti[2][3],206,189)
AngleList = []
VelocityList = []
traj = mr.CartesianTrajectory(Ti,Tf,10,iterations,3)
for i in range(iterations):
if i == 0:
Ik = mr.IKinSpace(Robot.Slist.T,Robot.M,traj[i],thetalist0, eomg = 0.00001, ev = 0.000001)
Vel = JointsVelocity(Robot.Slist,Ik,Robot.Vs)
VelocityList.append(Vel)
AngleList.append(Ik[0])
else:
Ik = mr.IKinSpace(Robot.Slist.T,Robot.M,traj[i],AngleList[i-1], eomg = 0.000001, ev = 0.000001)
Vel = JointsVelocity(Robot.Slist,Ik,Robot.Vs)
VelocityList.append(Vel)
AngleList.append(Ik[0])
return AngleList
def p7():
print('P7')
X_start = np.array([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
X_end = np.array([[0, 0, 1, 1],
[1, 0, 0, 2],
[0, 1, 0, 3],
[0, 0, 0, 1]])
Tf = 10
N = 10
method = 5
traj = mr.CartesianTrajectory(X_start, X_end, Tf, N, method)
ans = traj[-2]
print('second to last via configuration:\n%s' %ans)
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 createSegment8(current_state, goal_state, k):
totalSeconds = 100
N = float(totalSeconds) / float(k)
X_start = current_state.copy()
X_end = goal_state.copy()
# The total time in seconds
Tf = totalSeconds
# Method describes cubic or quintic scaling
method = 5
path_states = mr.CartesianTrajectory(X_start, X_end, Tf, N, method)
# Take the 3-D array and put it into a 2-D form
path_states = process_array(path_states, 0)
return path_states
def createSegment2(T_se_initial, T_sc_initial, T_ce_grasp, k):
totalSeconds = 100
N = float(totalSeconds) / float(k)
T_goal = np.matmul(T_sc_initial.copy(), T_ce_grasp.copy())
X_start = T_se_initial.copy()
X_end = T_goal.copy()
Tf = totalSeconds
# Method describes cubic or quintic scaling
method = 5
path_states = mr.CartesianTrajectory(X_start, X_end, Tf, N, method)
# Take the 3-D array and put it into a 2-D form
path_states = process_array(path_states, 0)
return path_states
def createSegment6(current_state, T_sc_final, T_ce_grasp, k):
goal_state = np.matmul(T_sc_final, T_ce_grasp)
totalSeconds = 200
N = float(totalSeconds) / float(k)
X_start = current_state.copy()
X_end = goal_state.copy()
# The total time in seconds
Tf = totalSeconds
# Method describes cubic or quintic scaling
method = 5
path_states = mr.CartesianTrajectory(X_start, X_end, Tf, N, method)
# Take the 3-D array and put it into a 2-D form
path_states = process_array(path_states, 1)
return path_states
import modern_robotics as mr
import numpy as np
import math
print mr.QuinticTimeScaling(5, 3)
Xstart = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
Xend = np.array([[0, 0, 1, 1], [1, 0, 0, 2], [0, 1, 0, 3], [0, 0, 0, 1]])
# tragectoryPoints = mr.ScrewTrajectory(Xstart, Xend, 10, 10, 3)
tragectoryPoints = mr.CartesianTrajectory(Xstart, Xend, 10, 10, 5)
with np.printoptions(precision=3, suppress=True):
print tragectoryPoints[8]
Tsc_i: The cube's initial configuration == Tsc_initial
Tsc_f: The cube's desired final configuration == Tsc_final
Tce_g: The end-effector's configuration relative to the cube when it is grasping the cube == Tce_grasp
Tce_s: The end-effector's standoff configuration above the cube, before and after grasping, relative to the cube == Tce_standoff
k: The number of trajectory reference configurations per 0.01 seconds
"""
Tse_si = np.dot(Tsc_i,Tce_s) # The end-effector's standoff configuration above the initial cube relative to the world frame
N = 2*k/0.01
traj1 = mr.CartesianTrajectory(Tse_i,Tse_si,2,N,5) # Move from initial position to the initial standoff position
for i in range(int(N)):
param.append([traj1[i][0][0],traj1[i][0][1],traj1[i][0][2],traj1[i][1][0],traj1[i][1][1],traj1[i][1][2],\
traj1[i][2][0],traj1[i][2][1],traj1[i][2][2],traj1[i][0][3],traj1[i][1][3],traj1[i][2][3],0])
Tse_gi = np.dot(Tsc_i,Tce_g) # The end-effector's configuration relative to the world when it is grasping the initial cube
traj2 = mr.CartesianTrajectory(Tse_si,Tse_gi,2,N,5) # Move from initial standoff position to grasp position
for i in range(int(N)):
param.append([traj2[i][0][0],traj2[i][0][1],traj2[i][0][2],traj2[i][1][0],traj2[i][1][1],traj2[i][1][2],\
traj2[i][2][0],traj2[i][2][1],traj2[i][2][2],traj2[i][0][3],traj2[i][1][3],traj2[i][2][3],0])
temp = param[-1]
temp[-1] = 1
for i in range(100):
param.append(temp)
traj3 = mr.CartesianTrajectory(Tse_gi,Tse_si,2,N,5) # Move from grasp position back to initial standoff position
for i in range(int(N)):
param.append([traj3[i][0][0],traj3[i][0][1],traj3[i][0][2],traj3[i][1][0],traj3[i][1][1],traj3[i][1][2],\
traj3[i][2][0],traj3[i][2][1],traj3[i][2][2],traj3[i][0][3],traj3[i][1][3],traj3[i][2][3],1])
Tse_sf = np.dot(Tsc_f,Tce_s) # The end-effector's standoff configuration above the final cube relative to the world frame
N = 5*k/0.01
def TrajectoryGenerator(E, C, F, G, S):
Xstart1 = np.copy(E) # Trajectory 1 Starting Point :- Tseinitial
Xend2 = np.dot(
C, G
) # Trajectory 2 Ending Point :- Tsg (Endeffector Poistion during Grasping)
Xend1 = np.dot(
C, S
) # Trajectory 1 Ending Point :- Tss (Endeffector Poistion during Standoff)
Xend6 = np.dot(F, G) # Tsg at Cube Goal (grasp)
Xend5 = np.dot(F, S) # Tss at Cube Goal(Standoff)
N2 = 200
t2 = N2 / 100 # Time and Number of Iteration for each Trajectory
N5 = 1600
t5 = N5 / 100
trac1 = mr.CartesianTrajectory(Xstart1, Xend1, 6, 600, 5)
trac2 = mr.CartesianTrajectory(Xend1, Xend2, t2, N2, 5)
trac4 = mr.CartesianTrajectory(Xend2, Xend1, t2, N2, 5)
trac5 = mr.CartesianTrajectory(Xend1, Xend5, t5, N5,
5) # Finding Trajectory using MR Function
trac6 = mr.CartesianTrajectory(Xend5, Xend6, t2, N2, 5)
trac8 = mr.CartesianTrajectory(Xend6, Xend5, t2, N2, 5)
matrix1 = np.zeros((600, 13))
matrix2 = np.zeros((N2, 13))
matrix3 = np.zeros((65, 13))
matrix4 = np.zeros((N2, 13))
matrix5 = np.zeros((N5, 13))
matrix6 = np.zeros((N2, 13))
matrix7 = np.zeros((65, 13))
matrix8 = np.zeros((N2, 13))
for i in range(600):
matrix1[i][0] = trac1[i][0][0]
matrix1[i][1] = trac1[i][0][1]
matrix1[i][2] = trac1[i][0][2]
matrix1[i][3] = trac1[i][1][0]
matrix1[i][4] = trac1[i][1][1]
matrix1[i][5] = trac1[i][1][2]
matrix1[i][6] = trac1[i][2][0]
matrix1[i][7] = trac1[i][2][1]
matrix1[i][8] = trac1[i][2][2]
matrix1[i][9] = trac1[i][0][3]
matrix1[i][10] = trac1[i][1][3]
matrix1[i][11] = trac1[i][2][3]
matrix1[i][12] = 0
for i in range(N2):
matrix2[i][0] = trac2[i][0][0]
matrix2[i][1] = trac2[i][0][1]
matrix2[i][2] = trac2[i][0][2]
matrix2[i][3] = trac2[i][1][0]
matrix2[i][4] = trac2[i][1][1]
matrix2[i][5] = trac2[i][1][2]
matrix2[i][6] = trac2[i][2][0]
matrix2[i][7] = trac2[i][2][1]
matrix2[i][8] = trac2[i][2][2]
matrix2[i][9] = trac2[i][0][3]
matrix2[i][10] = trac2[i][1][3]
matrix2[i][11] = trac2[i][2][3]
matrix2[i][12] = 0
for i in range(65):
matrix3[i, 0:12] = matrix2[N2 - 1, 0:12]
matrix3[i, 12] = 1
for i in range(N2):
matrix4[i][0] = trac4[i][0][0]
matrix4[i][1] = trac4[i][0][1]
matrix4[i][2] = trac4[i][0][2]
matrix4[i][3] = trac4[i][1][0]
matrix4[i][4] = trac4[i][1][1]
matrix4[i][5] = trac4[i][1][2]
matrix4[i][6] = trac4[i][2][0]
matrix4[i][7] = trac4[i][2][1]
matrix4[i][8] = trac4[i][2][2]
matrix4[i][9] = trac4[i][0][3]
matrix4[i][10] = trac4[i][1][3]
matrix4[i][11] = trac4[i][2][3]
matrix4[i][12] = 1
for i in range(N5):
matrix5[i][0] = trac5[i][0][0]
matrix5[i][1] = trac5[i][0][1]
matrix5[i][2] = trac5[i][0][2]
matrix5[i][3] = trac5[i][1][0]
matrix5[i][4] = trac5[i][1][1]
matrix5[i][5] = trac5[i][1][2]
matrix5[i][6] = trac5[i][2][0]
matrix5[i][7] = trac5[i][2][1]
matrix5[i][8] = trac5[i][2][2]
matrix5[i][9] = trac5[i][0][3]
matrix5[i][10] = trac5[i][1][3]
matrix5[i][11] = trac5[i][2][3]
matrix5[i][12] = 1
for i in range(N2):
matrix6[i][0] = trac6[i][0][0]
matrix6[i][1] = trac6[i][0][1]
matrix6[i][2] = trac6[i][0][2]
matrix6[i][3] = trac6[i][1][0]
matrix6[i][4] = trac6[i][1][1]
matrix6[i][5] = trac6[i][1][2]
matrix6[i][6] = trac6[i][2][0]
matrix6[i][7] = trac6[i][2][1]
matrix6[i][8] = trac6[i][2][2]
matrix6[i][9] = trac6[i][0][3]
matrix6[i][10] = trac6[i][1][3]
matrix6[i][11] = trac6[i][2][3]
matrix6[i][12] = 1
for i in range(65):
matrix7[i, 0:12] = matrix6[N2 - 1, 0:12]
matrix7[i, 12] = 0
for i in range(N2):
matrix8[i][0] = trac8[i][0][0]
matrix8[i][1] = trac8[i][0][1]
matrix8[i][2] = trac8[i][0][2]
matrix8[i][3] = trac8[i][1][0]
matrix8[i][4] = trac8[i][1][1]
matrix8[i][5] = trac8[i][1][2]
matrix8[i][6] = trac8[i][2][0]
matrix8[i][7] = trac8[i][2][1]
matrix8[i][8] = trac8[i][2][2]
matrix8[i][9] = trac8[i][0][3]
matrix8[i][10] = trac8[i][1][3]
matrix8[i][11] = trac8[i][2][3]
matrix8[i][12] = 0
finalmatrix = np.vstack(
(matrix1, matrix2, matrix3, matrix4, matrix5, matrix6, matrix7,
matrix8)) # Storing all 12 configuration and gripper state in Matrix
return finalmatrix
def trajectoryGenerator(T_se_initial, T_sc_initial, T_sc_final, T_ce_grasp,
T_ce_standoff, k):
# ---------------------------------
# segment 1: A trajectory to move the gripper from its initial configuration to a "standoff" configuration a few cm above the block
# represent the frame of end-effector when standoff in space frame
T_se_standoff_initial = T_sc_initial.dot(T_ce_standoff)
# generate trajectory when approaching the standoff position in segment 1
T_se_segment_1 = mr.CartesianTrajectory(T_se_initial,
T_se_standoff_initial, 5, 5 / 0.01,
3)
# ---------------------------------
# segment 2: A trajectory to move the gripper down to the grasp position
# represent the frame of end-effecto r in space frame in segment 2
T_se_grasp = T_sc_initial.dot(T_ce_grasp)
# generate trajectory when approaching the grasping position in segment 2
T_se_seg2 = mr.CartesianTrajectory(T_se_segment_1[-1], T_se_grasp, 2,
2 / 0.01, 3)
# append the trajectory of segment 2 after segment 1
T_se_before = np.append(T_se_segment_1, T_se_seg2, axis=0)
# ---------------------------------
# segment 3: Closing of the gripper
# append the trajectory of segment 3 by 63 times
for i in np.arange(64):
T_se_before = np.append(T_se_before,
np.array([T_se_before[-1]]),
axis=0)
# ---------------------------------
# segment 4: A trajectory to move the gripper back up to the "standoff" configuration
# generate trajectory when back on the standoff position in segment 4
T_se_segment_4 = mr.CartesianTrajectory(T_se_grasp, T_se_standoff_initial,
2, 2 / 0.01, 3)
# append the trajectory of segment 4
T_se_before = np.append(T_se_before, T_se_segment_4, axis=0)
# ---------------------------------
# segment 5: A trajectory to move the gripper to a "standoff" configuration above the final configuration
# generate trajectory when moving to the final standoff position in segment 5
T_se_standoff_final = T_sc_final.dot(T_ce_standoff)
T_se_segment_5 = mr.CartesianTrajectory(T_se_standoff_initial,
T_se_standoff_final, 8, 8 / 0.01,
3)
# append the trajectory of segment 5
T_se_before = np.append(T_se_before, T_se_segment_5, axis=0)
# ---------------------------------
# segment 6: A trajectory to move the gripper to the final configuration of the object
# generate the end-effector configuration when losing
T_se_lose = T_sc_final.dot(T_ce_grasp)
# generate trajectory when moving to the final cube position in segment 6
T_se_segment_6 = mr.CartesianTrajectory(T_se_standoff_final, T_se_lose, 2,
2 / 0.01, 3)
# append the trajectory of segment 6
T_se_before = np.append(T_se_before, T_se_segment_6, axis=0)
# ---------------------------------
# segment 7: Opening of the gripper
# append the trajectory of segment 7 by 63 times
for i in np.arange(64):
T_se_before = np.append(T_se_before,
np.array([T_se_before[-1]]),
axis=0)
# ---------------------------------
# segment 8: A trajectory to move the gripper back to the "standoff" configuration
# generate trajectory when moving to the final standoff position in segment 8
T_se_segment_8 = mr.CartesianTrajectory(T_se_before[-1],
T_se_standoff_final, 2, 2 / 0.01,
3)
# append the trajectory of segment 8
T_se_before = np.append(T_se_before, T_se_segment_8, axis=0)
# ---------------------------------
# generate a matrix which is n by 13
T_se_post = np.zeros([int(k * 21 / 0.01 + 64 * 2), 13])
# put the configuration, position and gripper state in matrix which is n by 13
for i in np.arange(int(k * 21 / 0.01 + 64 * 2)):
T_se_post[i, 0] = T_se_before[i, 0, 0]
T_se_post[i, 1] = T_se_before[i, 0, 1]
T_se_post[i, 2] = T_se_before[i, 0, 2]
T_se_post[i, 3] = T_se_before[i, 1, 0]
T_se_post[i, 4] = T_se_before[i, 1, 1]
T_se_post[i, 5] = T_se_before[i, 1, 2]
T_se_post[i, 6] = T_se_before[i, 2, 0]
T_se_post[i, 7] = T_se_before[i, 2, 1]
T_se_post[i, 8] = T_se_before[i, 2, 2]
T_se_post[i, 9] = T_se_before[i, 0, 3]
T_se_post[i, 10] = T_se_before[i, 1, 3]
T_se_post[i, 11] = T_se_before[i, 2, 3]
T_se_post[i, 12] = 0
# amend the gripper state in segment 3, 4, 5, 6
for i in np.arange(int(k * 7 / 0.01), int(k * 19 / 0.01 + 64)):
T_se_post[i, 12] = 1
return T_se_post
import numpy as np import math import modern_robotics as mr T = 5 t = 3 s = mr.QuinticTimeScaling(T, t) print(s) X_start = np.identity(4) X_end = np.array([[0, 0, 1, 1], [1, 0, 0, 2], [0, 1, 0, 3], [0, 0, 0, 1]]) T_f = 10 N = 10 method = 3 trajectory = mr.ScrewTrajectory(X_start, X_end, T_f, N, method) print(trajectory[-2]) method = 5 trajectory = mr.CartesianTrajectory(X_start, X_end, T_f, N, method) print(trajectory[-2])
import math
import numpy as np
import modern_robotics as mr
pi = math.pi
Tf = 5
t = 3
N = 10
s = mr.QuinticTimeScaling(Tf, t)
print("\nQuestion 5:\n", np.around(s, decimals=2))
X_start = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
X_end = np.array([[0, 0, 1, 1], [1, 0, 0, 2], [0, 1, 0, 3], [0, 0, 0, 1]])
Tf = 10
traj = mr.ScrewTrajectory(X_start, X_end, Tf, N, 3)
print("\nQuestion 6:\n",
np.array2string(np.around(traj[8], decimals=3), separator=','),
sep='')
traj = mr.CartesianTrajectory(X_start, X_end, Tf, N, 5)
print("\nQuestion 7:\n",
np.array2string(np.around(traj[8], decimals=3), separator=','),
sep='')
