def minJerkSetup_now(initial_angles, tf, waypoints, t_array=None):
num_waypoints = waypoints.shape[1]
try:
if t_array == None:
del_t = float(tf) / float(num_waypoints)
t_array = del_t * np.ones((num_waypoints, 1))
elif not t_array.size == num_waypoints:
raise ValueError('Time array length is incorrect')
elif not tf == np.sum(t_array):
raise ValueError('Time array must add up to final time')
except:
if not t_array.size == num_waypoints:
raise ValueError('Time array length is incorrect')
elif not tf == np.sum(t_array):
raise ValueError('Time array must add up to final time')
joint_constants = namedtuple('joint_constants', 'J1 J2 J3 J4 J5')
joint_const = joint_constants(np.zeros((6, num_waypoints)),
np.zeros((6, num_waypoints)),
np.zeros((6, num_waypoints)),
np.zeros((6, num_waypoints)),
np.zeros((6, num_waypoints)))
x0 = np.zeros((5, 6))
if initial_angles.ndim == 2:
if initial_angles.shape[0] == 5:
initial_angles = initial_angles.T
x0[:, 0] = initial_angles
x0[:, 3] = KMTCS.inverseKinematics(waypoints[0, 0], waypoints[1, 0],
waypoints[2, 0], waypoints[3, 0]).T
t0 = np.zeros((num_waypoints, 1))
tf = np.zeros((num_waypoints, 1))
tf[0] = t_array[0]
for i in range(num_waypoints):
if i > 0:
x0[:, 0] = x0[:, 3]
x0[:,
3] = KMTCS.inverseKinematics(waypoints[0, i], waypoints[1, i],
waypoints[2, i], waypoints[3, i]).T
t0[i] = tf[i - 1]
tf[i] = t0[i] + t_array[i]
joint_const.J1[:, i] = minJerkSetup(x0[0], t0[i], tf[i])
joint_const.J2[:, i] = minJerkSetup(x0[1], t0[i], tf[i])
joint_const.J3[:, i] = minJerkSetup(x0[2], t0[i], tf[i])
joint_const.J4[:, i] = minJerkSetup(x0[3], t0[i], tf[i])
joint_const.J5[:, i] = minJerkSetup(x0[4], t0[i], tf[i])
return joint_const
def generateJointTrajectory_now(time, traj_type='circle', tf=2.5):
theta = np.zeros((1, 1))
dt = 0.0001
angles = np.zeros((5, 5))
vel = np.zeros((3, 5))
if traj_type == 'circle':
c_x = 0.0
c_y = 0.3
c_z = 0.
radius = 0.095
for i in range(5):
peturb = dt * float(i - 2)
x, y, z = circle_now(c_x, c_y, c_z, radius, time + peturb, tf)
ang_temp = KMTCS.inverseKinematics(x, y, z, theta)
for j in range(5):
angles[i, j] = ang_temp[j]
for i in range(3):
vel[i] = (angles[:][i + 2] - angles[:][i]) / (2 * dt)
acc = (vel[2] - vel[0]) / (2 * dt)
else:
msg = 'Cannot generate trajectory for unknown type: ' + traj_type
raise ValueError(msg)
return angles[2], vel[1], acc
def generateJointTrajectory(posList, dt):
jointTrajectory = namedtuple('JointTrajectory',
['ang', 'vel', 'acc', 'time'])
theta = np.zeros(posList.x.shape)
jointTrajectory.ang = KMTCS.inverseKinematics(posList.x[0], posList.y[0],
posList.z[0], theta[0])
for i in range(1, posList.x.size):
angles = KMTCS.inverseKinematics(posList.x[i], posList.y[i],
posList.z[i], theta[i])
jointTrajectory.ang = np.append(jointTrajectory.ang, angles, axis=1)
jointTrajectory.vel = np.zeros(jointTrajectory.ang.shape)
jointTrajectory.acc = np.zeros(jointTrajectory.ang.shape)
for i in range(5):
jointTrajectory.vel[i] = differentiate(jointTrajectory.ang[i], dt)
jointTrajectory.acc[i] = differentiate(jointTrajectory.vel[i], dt)
return jointTrajectory
