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: