def path_planning(self, path_type='default'):
""" 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.01
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.traj_list = r.ScrewTrajectory(X_current, X_goal, Tf=Tf, N=N, method=QUINTIC)
self.set_init_frame() # save the robot's pose frame at start of the trajectory.
self.isPathPlanned = True
def screwTragectoryGenerator(T_start, T_end, motionTime, gripperState,
timeStep, K):
"""Generates a screw trajectory given a start and end configuration
:param T_start: start configuration (SE3 Matrix)
:param T_end: goal configuration (SE3 Matrix)
:param motionTime: Total time of motion
:param gripperState: Open / Close state of gripper. gripper state: 0 = open, 1 =
:param timeStep: A timestep delta_t
:param K: The number of trajectory reference configurations per timeStep
:return: Configuration Tse of the end-effector, expressed as 13 variables separated by commas. The 13 variables are, in order
r11, r12, r13, r21, r22, r23, r31, r32, r33, px, py, pz, gripper state
Generated number of trajectory points per second will be equal to K * timeStep
This method uses fifth-order polynomial time scaling for trajectory generation.
"""
trajectory = mr.ScrewTrajectory(T_start, T_end, motionTime,
motionTime * K / timeStep, 5)
formattedTragectory = []
for config in trajectory:
# Rounding off values to 8 decimal places
configuration = np.around(config, 8).tolist()
formattedConfiguration = configuration[0][0:3] + configuration[1][0:3] + configuration[2][0:3] + \
[configuration[0][3], configuration[1][3], configuration[2][3], gripperState]
formattedTragectory.append(formattedConfiguration)
return formattedTragectory
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 TrajectoryGenerator(Tse, Tsci, Tscf, Tcestand, Tcegrap, Tf_screw,
Tf_cartesian, k_per_centi_sec):
Tse = np.array(Tse)
Tsci = np.array(Tsci)
Tscf = np.array(Tscf)
Tcestand = np.array(Tcestand)
Tcegrap = np.array(Tcegrap)
gripper_state = 0
# segment 1: from starting to standoff
T1 = Tse
T2 = np.dot(Tsci, Tcestand)
Traj_seg = mr.ScrewTrajectory(T1, T2, Tf_screw,
100 * k_per_centi_sec * Tf_screw, 3)
Trajectory = [(T, gripper_state) for T in Traj_seg]
# segment 2: from standoff position (directly bove the block) to the grasping position
T1 = T2
T2 = np.dot(Tsci, Tcegrap)
Traj_seg = mr.ScrewTrajectory(T1, T2, Tf_cartesian,
100 * k_per_centi_sec * Tf_cartesian, 3)
# the operator '+=' will append the return value to Traj_seg list
Trajectory += [(T, gripper_state) for T in Traj_seg]
# segment 3: set end-effector to grasp
gripper_state = 1
Trajectory += [(T2, gripper_state) for _ in range(100 * k_per_centi_sec)]
# segment 4: return to standoff position
Ttemp = T1
T1 = T2
T2 = Ttemp
Traj_seg = mr.ScrewTrajectory(T1, T2, Tf_cartesian,
100 * k_per_centi_sec * Tf_cartesian, 3)
Trajectory += [(T, gripper_state) for T in Traj_seg]
# segment 5: to the final standoff position (dorectly above the target)
T1 = T2
T2 = np.dot(Tscf, Tcestand)
Traj_seg = mr.ScrewTrajectory(T1, T2, Tf_screw,
100 * k_per_centi_sec * Tf_screw, 3)
Trajectory += [(T, gripper_state) for T in Traj_seg]
# segment 6: from standoff position (directly bove the block) to the grasping position
T1 = T2
T2 = np.dot(Tscf, Tcegrap)
Traj_seg = mr.ScrewTrajectory(T1, T2, Tf_cartesian,
100 * k_per_centi_sec * Tf_cartesian, 3)
Trajectory += [(T, gripper_state) for T in Traj_seg]
# segment 7: set end-effector to release
gripper_state = 0
Trajectory += [(T2, gripper_state) for _ in range(100 * k_per_centi_sec)]
# segment 8: return to standoff position
Ttemp = T1
T1 = T2
T2 = Ttemp
Traj_seg = mr.ScrewTrajectory(T1, T2, Tf_cartesian,
100 * k_per_centi_sec * Tf_cartesian, 3)
Trajectory += [(T, gripper_state) for T in Traj_seg]
return Trajectory
def call_ScrewTrajectory(self,start, end, tf, N,order_method,gripper_state,trajectory_list):
"""
call the ScrewTrajectory from moder robotics
:param start: The initial end-effector configuration
:param end: The final end-effector configuration
:param Tf: Total time of the motion in seconds from rest to rest
:param N: The number of points N > 1 (Start and stop) in the discrete
representation of the trajectory
:param method: The time-scaling method, where 3 indicates cubic (third-
order polynomial) time scaling and 5 indicates quintic
(fifth-order polynomial) time scaling
:return: The discretized trajectory as a list of N matrices in SE(3)
separated in time by Tf/(N-1). The first in the list is Xstart
and the Nth is Xend
"""
trajectory = mr.ScrewTrajectory(start, end, tf, N, order_method)
functions.convertToCsvForm(trajectory_list,trajectory,gripper_state)
def p6():
print('P6')
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 = 3
traj = mr.ScrewTrajectory(X_start, X_end, Tf, N, method)
ans = traj[-2]
print('second to last via configuration:\n%s' %ans)
def TrajectoryGenerator():
# calls InitTG to assign these transformation matrices
Tse_initial, Tse_standoffInit, Tse_gripInit, Tse_standoffFinal, Tse_gripFinal = InitTG(
)
# Xstarts and Xends to iterate through as inputs to ScrewTrajectory
traj_iter = np.array([Tse_initial,Tse_standoffInit,Tse_gripInit,Tse_gripInit,Tse_standoffInit,Tse_standoffFinal, \
Tse_gripFinal,Tse_gripFinal, Tse_standoffFinal])
k = 1
grip_states = [0]
t = np.array([4, 1, 1, 1, 8, 1, 1,
1]) # durations for each segment of the total trajectory
trajectories = []
gState = 0
# iterating through the eight segments of the simulation
for i in range(0, len(traj_iter) - 1):
Xstart = traj_iter[i]
Xend = traj_iter[i + 1]
Tf = t[i]
N = k * Tf / 0.01
if i == 2 or i == 6:
gState = not gState
traj = mr.ScrewTrajectory(Xstart, Xend, Tf, N, 3)
for i in range(0, len(traj)):
if gState:
grip_states.append(1)
else:
grip_states.append(0)
trajectories.append(traj[i])
return trajectories, grip_states
def TrajectoryGenerator(Tse_initial, Tsc_initial, Tsc_final, Tce_grasp,
Tce_standoff, k, path_trajectory):
'''
Input:
Tse_initial - The initial configuration of the end-effector in the reference trajectory
Tsc_initial - 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_standoff - The end-effector's standoff configuration above the cube, before and after grasping, relative to the cube
k - The number of trajectory reference configurations per 0.01 second
Output:
csv file with trajectory references
For all the segments I calculate Tf and N first, then employ "ScrewTrajectory" function from the MR library.
For segments 3 and 7 (grasping and releasing) I simply write 70 equal configurations to make end-effector grip/release the cube
'''
method = 5 #The time-scaling method: 5 indicates quintic (5fth-order polynomial) time scaling.
#create csv file
traj_row = [
Tse_initial[0][0], Tse_initial[0][1], Tse_initial[0][2],
Tse_initial[1][0], Tse_initial[1][1], Tse_initial[1][2],
Tse_initial[2][0], Tse_initial[2][1], Tse_initial[2][2],
Tse_initial[0][3], Tse_initial[1][3], Tse_initial[2][3]
]
traj_row.append(0)
with open(path_trajectory, mode='w', newline='') as path_file:
path_writer = csv.writer(path_file,
delimiter=',',
quotechar='"',
quoting=csv.QUOTE_MINIMAL)
path_writer.writerow(traj_row)
#first segment
Tse_standoff = np.matmul(Tsc_initial, Tce_standoff)
Tf, N = calc_Tf(Tse_initial, Tse_standoff, k)
traj = mr.ScrewTrajectory(Tse_initial, Tse_standoff, Tf, N, method)
write_row(traj, 0, path_trajectory) #add gripper state 0
#second segment
#Xstart = Tse_standoff
Tse_grasp = np.matmul(Tsc_initial, Tce_grasp)
Tf, N = calc_Tf(Tse_standoff, Tse_grasp, k)
traj = mr.ScrewTrajectory(Tse_standoff, Tse_grasp, Tf, N, method)
write_row(traj, 0, path_trajectory) #add gripper state 0
#third segment
i = Tse_grasp
traj_row = [
i[0][0], i[0][1], i[0][2], i[1][0], i[1][1], i[1][2], i[2][0], i[2][1],
i[2][2], i[0][3], i[1][3], i[2][3]
]
traj_row.append(1) #add gripper state 1
for i in range(70):
write_files(traj_row, path_trajectory)
#fourth segment
Tf, N = calc_Tf(Tse_grasp, Tse_standoff, k)
traj = mr.ScrewTrajectory(Tse_grasp, Tse_standoff, Tf, N, method)
write_row(traj, 1, path_trajectory) #add gripper state 1
#fifth segment
Tse_standoff_final = np.matmul(Tsc_final, Tce_standoff)
Tf, N = calc_Tf(Tse_standoff, Tse_standoff_final, k)
traj = mr.ScrewTrajectory(Tse_standoff, Tse_standoff_final, Tf, N, method)
write_row(traj, 1, path_trajectory) #add gripper state 1
#sixth segment
Tse_grasp_final = np.matmul(Tsc_final, Tce_grasp)
Tf, N = calc_Tf(Tse_standoff_final, Tse_grasp_final, k)
traj = mr.ScrewTrajectory(Tse_standoff_final, Tse_grasp_final, Tf, N,
method)
write_row(traj, 1, path_trajectory) #add gripper state 1
#seventh segment
i = Tse_grasp_final
traj_row = [
i[0][0], i[0][1], i[0][2], i[1][0], i[1][1], i[1][2], i[2][0], i[2][1],
i[2][2], i[0][3], i[1][3], i[2][3]
]
traj_row.append(0) #add gripper state 0
for i in range(70):
write_files(traj_row, path_trajectory)
#eighth segment
Tf, N = calc_Tf(Tse_grasp_final, Tse_standoff_final, k)
traj = mr.ScrewTrajectory(Tse_grasp_final, Tse_standoff_final, Tf, N,
method)
write_row(traj, 0, path_trajectory) #add gripper state 0
def TrajectoryGenerator(Tse_initial, Tsc_initial, Tsc_final, Tce_grasp,
Tce_standoff, k):
z_standoff = Tce_standoff[2][3]
#Segment 1
z_base = Tsc_initial[2][3]
#print(Tsc_initial)
#print(z_standoff)
Tsc1_standoff = np.copy(Tsc_initial)
Tsc1_standoff[2][3] = z_base + z_standoff
#print(Tsc1_standoff)
Tse1_standoff = np.matmul(Tsc1_standoff, Tce_standoff)
#print(Tsc_initial)
Xstart1 = Tse_initial
Xend1 = Tse1_standoff
#Segment 2
Tse1_grasp = np.matmul(Tsc_initial, Tce_grasp)
Xstart2 = Tse1_standoff
Xend2 = Tse1_grasp
#print("Tsc_initial = ",Tsc_initial)
#print("Tse1_grasp = ",Xend2)
#segment 3
Xstart3 = Tse1_grasp
Xend3 = Tse1_grasp
#segment 4
Xstart4 = Tse1_grasp
Xend4 = Tse1_standoff
#Segment 5
Tsc2_standoff = np.copy(Tsc_final)
Tsc2_standoff[2][3] = z_base + z_standoff
Tse2_standoff = np.matmul(Tsc2_standoff, Tce_standoff)
Xstart5 = Tse1_standoff
Xend5 = Tse2_standoff
#Segment 6
Tse2_grasp = np.matmul(Tsc_final, Tce_grasp)
Xstart6 = Tse2_standoff
Xend6 = Tse2_grasp
#Segment 7
Xstart7 = Tse2_grasp
Xend7 = Tse2_grasp
#Segment 8
Xstart8 = Tse2_grasp
Xend8 = Tse2_standoff
#Screw Trajectory Calculation for 1-8 segments
Tf = k
T1 = 4
T2 = 2
T3 = 1
T4 = T2
T5 = T1
T6 = T2
T7 = T3
T8 = T2
N1 = T1 * Tf / 0.01
N2 = T2 * Tf / 0.01
N3 = T3 * Tf / 0.01
N4 = T4 * Tf / 0.01
N5 = T5 * Tf / 0.01
N6 = T6 * Tf / 0.01
N7 = T7 * Tf / 0.01
N8 = T8 * Tf / 0.01
method = 5
lst = []
g_state = 0
output1 = mr.ScrewTrajectory(Xstart1, Xend1, Tf, N1, method)
lst = create_lst(output1, lst, g_state)
g_state = 0
output2 = mr.ScrewTrajectory(Xstart2, Xend2, Tf, N2, method)
lst = create_lst(output2, lst, g_state)
g_state = 1
output3 = mr.ScrewTrajectory(Xstart3, Xend3, Tf, N3, method)
lst = create_lst(output3, lst, g_state)
g_state = 1
output4 = mr.ScrewTrajectory(Xstart4, Xend4, Tf, N4, method)
lst = create_lst(output4, lst, g_state)
g_state = 1
output5 = mr.ScrewTrajectory(Xstart5, Xend5, Tf, N5, method)
lst = create_lst(output5, lst, g_state)
g_state = 1
output6 = mr.ScrewTrajectory(Xstart6, Xend6, Tf, N6, method)
lst = create_lst(output6, lst, g_state)
g_state = 1
output7 = mr.ScrewTrajectory(Xstart7, Xend7, Tf, N7, method)
lst = create_lst(output7, lst, g_state)
g_state = 0
output8 = mr.ScrewTrajectory(Xstart8, Xend8, Tf, N8, method)
lst = create_lst(output8, lst, g_state)
# f = open("output.csv", "w")
# lst = [output1, output2, output3, output4, output5, output6, output7, output8]
# num = 1
# for out in lst:
# if num >= 3 and num <= 7:
# gripper_state = 1
# else:
# gripper_state = 0
# for i in out:
# write_csv(i,gripper_state, f)
# num=num+1
# f.close()
#output = output1+output2+output3+output4+output5+output6+output7+output8
#print(lst)
#print(len(lst))
return lst
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])
def get_traj(X_s, X_e):
return mr.ScrewTrajectory(X_s, X_e, 5, 3, 3)
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='')
def milestone3_test4_debug():
"""
Milestone 3 : Test4
Check robot controller for generated tragectory (kp = 0, ki = 0)
"""
# configuration = [0, 0, 0, 0, -0.35, -0.698, -0.505, 0, 0, 0, 0, 0]
configuration = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
Tsc_initial = [[1, 0, 0, 1], [0, 1, 0, 0], [0, 0, 1, 0.025], [0, 0, 0, 1]]
Tsc_goal = [[0, 1, 0, 0], [-1, 0, 0, -1], [0, 0, 1, 0.025], [0, 0, 0, 1]]
Tse_initial = kinematic_model.youBot_Tse(configuration)
result_configuration = []
result_Xerr = []
gains = [0, 0] # kp, ki
timeStep = 0.5
K = 1
# Generating stadoff and gripping transforms
theta = [0, 3 * math.pi / 4, 0]
R_so3 = mr.VecToso3(theta)
R = mr.MatrixExp3(R_so3)
position = [-0.01, 0, 0.20]
Tce_standoff = mr.RpToTrans(R, position)
position = [-0.01, 0, 0.01]
Tce_gripping = mr.RpToTrans(R, position)
# Trajectory generation
# trajectory = trajectory_planner.TrajectoryGenerator(Tse_initial, Tsc_initial, Tsc_goal, Tce_gripping, Tce_standoff, timeStep, K )
Xstart = kinematic_model.youBot_Tse(configuration)
XendConfig = [0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Xend = kinematic_model.youBot_Tse(XendConfig)
trajectory = mr.ScrewTrajectory(Xstart, Xend, 3, 6, 3)
csv_writer.writeDataList(trajectory, "milestone3_trajectory")
# Control generation
for x in range(len(trajectory) - 1):
Xd = trajectory[x]
Xd_next = trajectory[x + 1]
# Xd = kinematic_model.configurationToTransform(trajectory[x])
# Xd_next = kinematic_model.configurationToTransform(trajectory[x+1])
Tse = kinematic_model.youBot_Tse(configuration)
print "####### Iteration #######"
print "Tse"
print np.around(Tse, 3).tolist()
print "Xd"
print np.around(Xd, 3).tolist()
print "Xd_next"
print np.around(Xd_next, 3).tolist()
V, Xerr = controller.FeedbackControl(Tse, Xd, Xd_next, gains, timeStep)
print "V"
print np.around(V, 3).tolist()
print "Xerr"
print np.around(Xerr, 3).tolist()
# Calculate Je
Je = kinematic_model.youBot_Je(configuration)
while True:
# Calculating control
cmd = np.dot(np.linalg.pinv(Je), V)
# Control : omega (5 variables), wheel speeds u (4 variables)
control = np.concatenate((cmd[4:9], cmd[0:4]))
print "control"
print np.around(control, 3).tolist()
next_config = kinematic_simulator.NextState(
configuration, control, timeStep)
print "next_config"
print np.around(next_config, 3).tolist()
# Joint limit checking
status, jointVector = kinematic_model.youBot_testJointLimits(
next_config[3:8])
if status:
configuration = next_config
break
else:
Je[:, 4:9] = Je[:, 4:9] * jointVector
# Save configuration (Tse) and Xerr per every K iterations
if (x % K) == 0:
result_configuration.append(configuration)
result_Xerr.append(Xerr)
# print np.around(configuration, 3).tolist()
csv_writer.writeDataList(result_configuration, "milestone3_configurations")