def FeedbackControl(X, X_d, X_d_next, K_p, K_i, dt):
global runningError # This is the integral error over time (0, now)
global updateCount # Describes if we should store the
# Current point on trajectory's error
# Compute the desired twist using Modern Robotics 13.4 and 13.5
AdjointResult = mr.Adjoint(np.matmul(mr.TransInv(X), X_d))
v_d = (1.0 / dt) * (mr.MatrixLog6(np.matmul(mr.TransInv(X_d), X_d_next)))
v_d = mr.se3ToVec(v_d)
X_err = mr.MatrixLog6(np.matmul(mr.TransInv(X), X_d))
X_err = mr.se3ToVec(X_err)
# Store every kth error term
if (updateCount == 2):
updateError(X_err)
updateCount = 0
else:
updateCount = updateCount + 1
# Increment our integral error term
runningError = runningError + (X_err * dt)
return np.matmul(AdjointResult, v_d) + np.matmul(K_p, X_err) + np.matmul(
K_i, runningError)
def Feedforward_Control(self,X,Xd,Xd_next):
"""
calculate the joint velocitys
based on the current eef configuration and the next(t+dt) eef configuration
Input:
The current actual end-effector configuration X
The current end-effector reference configuration Xd
The end-effector reference configuration at the next timestep in the reference trajectory,Xd_next at a time Δt later.
Return:
The eef twist and error
"""
#calculate X error
X_err = mr.MatrixLog6(np.matmul(mr.TransInv(X),Xd))
X_next_err = mr.MatrixLog6(np.matmul(mr.TransInv(Xd), Xd_next))
#calculate Vd
Vd = mr.se3ToVec(1/self.dt * X_next_err )
#calculate intergal error (kepp the sum in variable error_intergal)
self.error_intergal = self.error_intergal + X_err*self.dt
#calculate eef velocity twist
AdJoint_mat = mr.Adjoint(np.matmul(mr.TransInv(X),Xd))
Kp_var = mr.se3ToVec(np.matmul(self.Kp,X_err))
Ki_var = mr.se3ToVec(np.matmul(self.Ki,self.error_intergal))
V = np.matmul(AdJoint_mat,Vd) + Kp_var + Ki_var
return V, mr.se3ToVec(X_err)
def IKinBodyIterates(Blist, M, T, thetalist0, eomg, ev):
"""
Computes inverse kinematics in the body frame for an open chain robot and saves the report in a log.txt file
:param Blist: The joint screw axes in the end-effector frame when the
manipulator is at the home position, in the format of a
matrix with axes as the columns
:param M: The home configuration of the end-effector
:param T: The desired end-effector configuration Tsd
:param thetalist0: An initial guess of joint angles that are close to
satisfying Tsd
:param eomg: A small positive tolerance on the end-effector orientation
error. The returned joint angles must give an end-effector
orientation error less than eomg
:param ev: A small positive tolerance on the end-effector linear position
error. The returned joint angles must give an end-effector
position error less than ev
:return thetalist: Joint angles that achieve T within the specified
tolerances,
:return success: A logical value where TRUE means that the function found
a solution and FALSE means that it ran through the set
number of maximum iterations without finding a solution
within the tolerances eomg and ev.
Uses an iterative Newton-Raphson root-finding method.
The maximum number of iterations before the algorithm is terminated has
been hardcoded in as a variable called maxiterations. It is set to 20 at
the start of the function, but can be changed if needed.
"""
thetalist = np.array(thetalist0).copy()
thetamatrix = thetalist.T
i = 0
maxiterations = 20
Tsb = mr.FKinBody(M, Blist, thetalist)
Vb = mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(Tsb), T)))
omg_b_norm = np.linalg.norm([Vb[0], Vb[1], Vb[2]])
v_b_norm = np.linalg.norm([Vb[3], Vb[4], Vb[5]])
err = omg_b_norm > eomg or v_b_norm > ev
saveLog(i, thetalist, Tsb, Vb, omg_b_norm, v_b_norm)
while err and i < maxiterations:
thetalist = thetalist \
+ np.dot(np.linalg.pinv(mr.JacobianBody(Blist, \
thetalist)), Vb)
thetamatrix = np.vstack((thetamatrix, thetalist.T))
i = i + 1
Tsb = mr.FKinBody(M, Blist, thetalist)
Vb = mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(Tsb), T)))
omg_b_norm = np.linalg.norm([Vb[0], Vb[1], Vb[2]])
v_b_norm = np.linalg.norm([Vb[3], Vb[4], Vb[5]])
err = omg_b_norm > eomg or v_b_norm > ev
# print details to log.txt
saveLog(i, thetalist, Tsb, Vb, omg_b_norm, v_b_norm, isappend="a")
np.savetxt("iterates.csv", thetamatrix, delimiter=",")
return (thetamatrix, not err)
def Essential_function(phi, x, y, a1, a2, a3, a4, a5):
Tsb_q = np.array([[cos(phi), -sin(phi), 0, x], [sin(phi), cos(phi), 0, y], [0, 0, 1, 0.0963], [0, 0, 0, 1]])
thetalist = np.array([a1, a2, a3, a4, a5])
T0e_theta = mr.FKinBody(M0e, Blist, thetalist)
x_q_theta = np.dot(np.dot(Tsb_q, Tb_0), T0e_theta)
jarm = mr.JacobianBody(Blist, thetalist)
F = np.array([[0, 0, 0, 0], [0, 0, 0, 0],[-1/(l+w), 1/(l+w), 1/(l+w), -1/(l+w)], [1, 1, 1, 1], [-1, 1, -1, 1], [0, 0, 0, 0]])
jbase = np.dot(mr.Adjoint(np.dot(mr.TransInv(T0e_theta), mr.TransInv(Tb_0))), F)
return x_q_theta, jarm, jbase #X, Jb, Jbase is return from here
def calc_joint_vel(self):
rospy.logdebug("Calculating joint velocities...")
# Convert Slist to Blist
Tbs = r.TransInv(self.M0)
Blist = np.zeros(self.Slist.shape) #6x7 mx of 0's
for item, screw in enumerate(self.Slist.T): #index = 7, screw = 6x1
Blist[:, item] = np.dot(r.Adjoint(Tbs), screw.T)
# Current joint angles
self.get_q_now()
with self.mutex:
q_now = self.q #1x7 mx
# Desired config: base to desired - Tbd
with self.mutex:
T_sd = self.T_goal
# Find transform from current pose to desired pose, error
e = np.dot(r.TransInv(r.FKinBody(self.M0, Blist, q_now)), T_sd)
# Desired TWIST: MatrixLog6 SE(3) -> se(3) exp coord
Vb = r.se3ToVec(r.MatrixLog6(e))
# Construct BODY JACOBIAN for current config
Jb = r.JacobianBody(Blist, q_now) #6x7 mx
# Desired ang vel - Eq 5 from Chiaverini & Siciliano, 1994
# Managing singularities: naive least-squares damping
n = Jb.shape[-1] #Size of last row, n = 7 joints
invterm = np.linalg.inv(np.dot(Jb.T, Jb) + pow(self.damping, 2)*np.eye(n))
qdot_new = np.dot(np.dot(invterm,Jb.T),Vb)
# Scaling joint velocity
minus_v = abs(np.amin(qdot_new))
plus_v = abs(np.amax(qdot_new))
if minus_v > plus_v:
scale = minus_v
else:
scale = plus_v
if scale > self.joint_vel_limit:
qdot_new = 1.0*(qdot_new/scale)*self.joint_vel_limit
self.qdot = qdot_new #1x7
# Constructing dictionary
qdot_output = dict(zip(self.names, self.qdot))
# Setting Baxter right arm joint velocities
self.arm.set_joint_velocities(qdot_output)
return
def calc_jacobian(self,q):
"""
calculating the jacobian matrix
"""
T0e = mr.FKinBody(self.allMatrix.M0e,
self.allMatrix.Blist,
q[3:])
Teb = np.matmul(mr.TransInv(T0e),mr.TransInv(self.allMatrix.Tb0))
Ad = mr.Adjoint(Teb)
Jbase = np.matmul(Ad,self.allMatrix.F6)
J_arm = mr.JacobianBody(self.allMatrix.Blist, q[3:])
J_e = np.concatenate((Jbase, J_arm),1)
Je_pinv = np.linalg.pinv(J_e,1e-4)
return Je_pinv, mr.TransInv(Teb)
def step_ik(self):
# grab current joint angles and SE(3) target:
q = self.q
with self.mutex:
g_sd = self.goal
# Calculate transform from current EE pose to desired EE pose
err = np.dot(mr.TransInv(mr.FKinBody(smr.M, smr.Blist, q)), g_sd)
# now convert this to a desired twist
Vb = mr.se3ToVec(mr.MatrixLog6(err))
# calculate body Jacobian at current config
J_b = mr.JacobianBody(smr.Blist, q)
# now calculate an appropriate joint angle velocity:
# qdot = np.dot(np.linalg.pinv(J_b), Vb)
idim = J_b.shape[-1]
qdot = np.dot(np.dot(np.linalg.inv(np.dot(J_b.T,J_b) + self.damping*np.eye(idim)), J_b.T), Vb)
# increase velocity in each joint
multiplier = 4
# print np.amax(qdot), " --to--> ", np.amax(qdot*multiplier)
qdot = qdot*multiplier
# clip down velocity in each joint
if np.amax(qdot) > self.joint_vel_limit:
qdot = qdot/np.amax(qdot)*self.joint_vel_limit
names = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6']
# print "max vel joint : ", names[qdot.index(np.amax(qdot))], " : ", qdot[qdot.index(np.amax(qdot))]
# print "max vel joint : ", names[np.where(qdot == np.amax(qdot))[0]], " : ", qdot[np.where(qdot == np.amax(qdot))[0]]
if np.amax(qdot) > self.joint_vel_limit:
print "joint limit reach !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
self.q_sim += self.step_scale*qdot
self.joint_cmd.velocity = qdot
return
def calc_joint_vel_2(self):
""" Joint velocity command computation based on PI feedback loop.
"""
Tbs = self.M0 #
Blist = self.Blist #
self.get_q_now()
with self.mutex:
q_now = self.q #1x7 mx
with self.mutex:
T_sd = self._get_goal_matrix() #
dT = self.get_dt()
e = np.dot(r.TransInv(r.FKinBody(Tbs, Blist, q_now)), T_sd) # Modern robotics pp 230 22Nff
# get Error vector.
Xe = r.se3ToVec(r.MatrixLog6(e)) # shape -> [1, 2, 3, 4, 5, 6]
dXe = Xe - self.last_error
# numerical integration of the error
if np.linalg.norm(self.int_err) < self.int_anti_windup:
self.int_err += Xe # * dT
# numerical differentiation of the error
if dT > 0:
self.der_err = dXe / dT
Vb = np.dot(self.Kp, Xe) + np.dot(self.Ki, self.int_err) #+ np.dot(self.Kd, self.der_err) # Kp*Xe + Ki*intg(Xe) + Kd*(dXe/dt)
Vb += self.get_feed_forward()
# self.V_b = np.dot(self.Kp, self.X_e) + np.dot(self.Ki, self.int_err)
self.J_b = r.JacobianBody(Blist, self.q)
n = self.J_b.shape[-1] #Size of last row, n = 7 joints
invterm = np.linalg.inv(np.dot(self.J_b.T, self.J_b) + pow(self.damping, 2)*np.eye(n)) #
self.q_dot = np.dot(np.dot(invterm, self.J_b.T),Vb) # It seems little bit akward...? >>Eq 6.7 on pp 233 of MR book
self.q_dot = self.scale_joint_vel(self.q_dot)
qdot_output = dict(zip(self.names, self.q_dot))
self.limb.set_joint_velocities(qdot_output)
# self.stop_oscillating()
self.last_error = Xe
self.last_time = self.current_time
self._check_traj(dT)
def step_ik(self,tdat):
with self.mutex:
g_sd = self.goal
current_js = self.js
# Calculate transform from current EE pose to desired EE pose
err = np.dot(mr.TransInv(mr.FKinBody(smr.M, smr.Blist, current_js)), g_sd)
# now convert this to a desired twist
Vb = mr.se3ToVec(mr.MatrixLog6(err))
if (self.ik_begin == True and self.xPos < .55 and self.xPos > -.7):
Vb[3] = self.vely
elif (self.ik_begin == True and self.xPos > .55 or self.xPos < -.7):
self.ik_begin = False
self.slow = True
if (self.vely == 0):
self.ik_begin = False
self.slow = True
if (self.count < pi/2 and self.slow == True):
Vb[3] = self.xdot*cos(self.count) + Vb[3]*sin(self.count/4)
self.count = self.count + .015
elif (self.count >= pi/2 and self.count < 2*pi and self.slow == True):
Vb[3] = Vb[3]*sin(self.count/4)
self.count = self.count + .015
elif (self.count >= 2*pi and self.slow == True):
self.count = 0
self.slow = False
# calculate body Jacobian at current config
J_b = mr.JacobianBody(smr.Blist, current_js)
# now calculate an appropriate joint angle velocity:
qdot = np.dot(np.linalg.pinv(J_b), Vb)
joint_command = {"right_j0":qdot[0], "right_j1":qdot[1], "right_j2":qdot[2],
"right_j3": qdot[3], "right_j4":qdot[4], "right_j5":qdot[5], "right_j6":qdot[6]}
self.limb.set_joint_velocities(joint_command)
def step_ik(self):
# grab current joint angles and SE(3) target:
q = self.q
with self.mutex:
g_sd = self.goal
# Calculate transform from current EE pose to desired EE pose
err = np.dot(mr.TransInv(mr.FKinBody(smr.M, smr.Blist, q)), g_sd)
# now convert this to a desired twist
Vb = mr.se3ToVec(mr.MatrixLog6(err))
# calculate body Jacobian at current config
J_b = mr.JacobianBody(smr.Blist, q)
# now calculate an appropriate joint angle velocity:
# qdot = np.dot(np.linalg.pinv(J_b), Vb)
idim = J_b.shape[-1]
qdot = np.dot(np.dot(np.linalg.inv(np.dot(J_b.T,J_b) + self.damping*np.eye(idim)), J_b.T), Vb)
# increase velocity in each joint
multiplier = 4
qdot = qdot*multiplier
########Clip down algorithm
# joint vel lim dict
vel_lim = {'right_j0': 1.74, 'right_j1': 1.328, 'right_j2': 1.957, 'right_j3': 1.957, 'right_j4': 3.485, 'right_j5': 3.485, 'right_j6': 4.545}
qdot_unclip = dict(zip(self.joint_names, qdot))
qd_over_vel = [qd for qd in qdot_unclip if qdot_unclip[qd] > vel_lim[qd]]
if qd_over_vel != []:
print "joint limit reach in: ", qd_over_vel
qd_vel_lim_min = min(qd_over_vel, key=qd_over_vel.get)
f = 0.7
qdot = qdot/qdot_unclip[qd_vel_lim_min]*vel_lim[qd_vel_lim_min]*f
print "reduce to: ", qdot
print "with limit: ", vel_lim
self.qdot_dict = dict(zip(self.joint_names, qdot))
def FeedbackControl(Xd, Xd_next, X, Kp, Ki, dt, Jarm, Jbase):
global add_err
Vd_se3 = mr.MatrixLog6(np.dot(mr.TransInv(Xd), Xd_next))*(1/dt)
Vd = mr.se3ToVec(Vd_se3)
x_in_xd = np.dot(mr.TransInv(X), Xd) #X^-1*X_d
X_error_se3 = mr.MatrixLog6(x_in_xd)
X_error = mr.se3ToVec(X_error_se3)
adjoint = mr.Adjoint(x_in_xd)
forward_control = np.dot(adjoint,Vd) #Forward controller term
proportional = np.dot(Kp, X_error) #proportional controller term
add_err = add_err+X_error*dt #for integral error adding
integral = np.dot(Ki, add_err)
V = forward_control+proportional+integral
Je = np.append(Jbase, Jarm, axis=1)
velocity = np.dot(np.linalg.pinv(Je, rcond=1e-4), V)
return velocity, X_error
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 p11():
print('P11')
T = np.array([[0, -1, 0, 3],
[1, 0, 0, 0],
[0, 0, 1, 1],
[0, 0, 0, 1]])
result = mr.TransInv(T)
print(result)
def gen_joints_list(self, input_pose):
self.limb_interface.move_to_joint_positions({'head_pan':0.379125, 'right_j0':-0.020025390625 , 'right_j1':0.8227529296875, 'right_j2':-2.0955126953125, 'right_j3':2.172509765625, 'right_j4':0.7021171875, 'right_j5' : -1.5003603515625, 'right_j6' : -2.204990234375})
# self.running_flag = True
tol = 0.001 #1 mm
ret_list = []
while (abs(self.ep.pose.position.x - input_pose.position.x) > tol) and (abs(self.ep.pose.position.y - input_pose.position.y) > tol):
# set_goal_from_pose part
p = np.array([input_pose.position.x, input_pose.position.y,
input_pose.position.z])
q = np.array([input_pose.orientation.x, input_pose.orientation.y,
input_pose.orientation.z, input_pose.orientation.w])
goal = tr.euler_matrix(*tr.euler_from_quaternion(q))
goal[0:3,-1] = p
# actual_js_cb: get the actual joint state
q = self.q
with self.mutex:
g_sd = goal
# Calculate transform from current EE pose to desired EE pose
err = np.dot(mr.TransInv(mr.FKinBody(smr.M, smr.Blist, q)), g_sd)
# now convert this to a desired twist
Vb = mr.se3ToVec(mr.MatrixLog6(err))
# calculate body Jacobian at current config
J_b = mr.JacobianBody(smr.Blist, q)
# now calculate an appropriate joint angle velocity:
# qdot = np.dot(np.linalg.pinv(J_b), Vb)
idim = J_b.shape[-1]
qdot = np.dot(np.dot(np.linalg.inv(np.dot(J_b.T,J_b) + self.damping*np.eye(idim)), J_b.T), Vb)
# increase velocity in each joint
multiplier = 4
qdot = qdot*multiplier
########Clip down algorithm
# joint vel lim dict
vel_lim = {'right_j0': 1.74, 'right_j1': 1.328, 'right_j2': 1.957, 'right_j3': 1.957, 'right_j4': 3.485, 'right_j5': 3.485, 'right_j6': 4.545}
qdot_unclip = dict(zip(self.joint_names, qdot))
qd_over_vel = [[qd, qdot_unclip[qd]] for qd in qdot_unclip if qdot_unclip[qd] > vel_lim[qd]]
if qd_over_vel != []:
print "joint limit reach in: ", qd_over_vel
# qd_over_vel = dict(zip(qd_over_vel, qdot_unclip[qd]))
# qd_vel_lim_min = min(qd_over_vel, key=qd_over_vel.get)
qd_vel_lim_val = [qd_over_vel[i][1] for i in range(len(qd_over_vel))]
qd_vel_lim_min = min(qd_vel_lim_val)
f = 0.7
# qdot = qdot/qdot_unclip[qd_vel_lim_min]*vel_lim[qd_vel_lim_min]*f
qdot = qdot/qd_vel_lim_min*qd_vel_lim_min*f
print "reduce to: ", qdot
print "with limit: ", vel_lim
qdot_dict = dict(zip(self.joint_names, qdot))
self.limb_interface.set_joint_velocities(qdot_dict)
print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1\n", qdot_dict
ret_list.append(qdot_dict)
self.limb_interface.move_to_joint_positions({'head_pan':0.379125, 'right_j0':-0.020025390625 , 'right_j1':0.8227529296875, 'right_j2':-2.0955126953125, 'right_j3':2.172509765625, 'right_j4':0.7021171875, 'right_j5' : -1.5003603515625, 'right_j6' : -2.204990234375})
return ret_list
def calculate_desired_twist(Tse, Tsed, Tsd, Kp, Ki, Kd, dt): # Tse: is the current actual end_effector frame # Tsed: is the current referrance end_effector frame on the trajectory # Tsd: is the next referrance end_effector frame on the trajectory # Kp, Ki: the proportional and the integrator gain # Kd: usually 1 or 0 to switch the feedforward on or off Tse = np.array(Tse) Tsed = np.array(Tsed) Tsd = np.array(Tsd) Xerr_se3 = mr.MatrixLog6(mr.TransInv(Tse) @ Tsed) Xerr = mr.se3ToVec(Xerr_se3) global cdt_integrator cdt_integrator += dt * Xerr # if kd = 0, then feedforward Vd = 0 Vd = (1/dt) * mr.se3ToVec(mr.MatrixLog6(mr.TransInv(Tsed) @ Tsd)) if Kd != 0 else np.zeros((6,)) # feedforward + feedback V = (Kd * mr.Adjoint(mr.TransInv(Tse) @ Tsed) @ Vd) + Kp*Xerr + Ki*cdt_integrator return V, Xerr
def IKinBodyIterates(Blist, M, T, thetalist0, eomg, ev):
"""
This function was cloned from the Modern Robotics library
Modifications to the function track robot configuration and error
"""
# Store information to track changes (this is new)
# new information is put into an array
j_history = []
se3_history = []
error_twist_history = []
ang_err_mag_history = []
lin_err_mag_history = []
thetalist = np.array(thetalist0).copy()
i = 0
maxiterations = 20
Vb = mr.se3ToVec(
mr.MatrixLog6(np.dot(mr.TransInv(mr.FKinBody(M, Blist, thetalist)),
T)))
err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg or np.linalg.norm(
[Vb[3], Vb[4], Vb[5]]) > ev
while err and i < maxiterations:
# changes are appended to corresponding array
j_history.append(thetalist)
se3_history.append(mr.FKinBody(M, Blist, thetalist))
error_twist_history.append(Vb)
ang_err_mag_history.append(np.linalg.norm([Vb[0], Vb[1], Vb[2]]))
lin_err_mag_history.append(np.linalg.norm([Vb[3], Vb[4], Vb[5]]))
thetalist = thetalist + np.dot(
np.linalg.pinv(mr.JacobianBody(Blist, thetalist)), Vb)
i = i + 1
Vb = mr.se3ToVec(
mr.MatrixLog6(
np.dot(mr.TransInv(mr.FKinBody(M, Blist, thetalist)), T)))
err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg or np.linalg.norm(
[Vb[3], Vb[4], Vb[5]]) > ev
return (thetalist, not err, j_history, se3_history, error_twist_history,
ang_err_mag_history, lin_err_mag_history)
def IKinBodyIterates(Blist, M, T, thetalist0, eomg, ev):
thetalist = np.array(thetalist0).copy()
i = 0
maxiterations = 20
Vb = mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(mr.FKinBody(M, Blist, \
thetalist)), T)))
err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \
or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev
while err and i < maxiterations:
thetalist = thetalist \
+ np.dot(np.linalg.pinv(mr.JacobianBody(Blist, \
thetalist)), Vb)
i = i + 1
Vb \
= mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(mr.FKinBody(M, Blist, \
thetalist)), T)))
err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \
or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev
return (thetalist, not err, i)
def set_ee_cartesian_trajectory(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, moving_time=None, wp_moving_time=0.2, wp_accel_time=0.1, wp_period=0.05):
if self.group_info.num_joints < 6 and (y != 0 or yaw != 0):
rospy.loginfo("Please leave the 'y' and 'yaw' fields at '0' when working with arms that have less than 6dof.")
return False
rpy = ang.rotationMatrixToEulerAngles(self.T_sb[:3,:3])
T_sy = np.identity(4)
T_sy[:3,:3] = ang.eulerAnglesToRotationMatrix([0.0, 0.0, rpy[2]])
T_yb = np.dot(mr.TransInv(T_sy), self.T_sb)
rpy[2] = 0.0
if (moving_time == None):
moving_time = self.moving_time
accel_time = self.accel_time
N = int(moving_time / wp_period)
inc = 1.0 / float(N)
joint_traj = JointTrajectory()
joint_positions = list(self.joint_commands)
for i in range(N+1):
joint_traj_point = JointTrajectoryPoint()
joint_traj_point.positions = joint_positions
joint_traj_point.time_from_start = rospy.Duration.from_sec(i * wp_period)
joint_traj.points.append(joint_traj_point)
if (i == N):
break
T_yb[:3,3] += [inc * x, inc * y, inc * z]
rpy[0] += inc * roll
rpy[1] += inc * pitch
rpy[2] += inc * yaw
T_yb[:3,:3] = ang.eulerAnglesToRotationMatrix(rpy)
T_sd = np.dot(T_sy, T_yb)
theta_list, success = self.set_ee_pose_matrix(T_sd, joint_positions, False, blocking=False)
if success:
joint_positions = theta_list
else:
rospy.loginfo("%.1f%% of trajectory successfully planned. Trajectory will not be executed." % (i/float(N) * 100))
break
if success:
self.set_trajectory_time(wp_moving_time, wp_accel_time)
joint_traj.joint_names = self.group_info.joint_names
current_positions = []
with self.core.js_mutex:
for name in joint_traj.joint_names:
current_positions.append(self.core.joint_states.position[self.core.js_index_map[name]])
joint_traj.points[0].positions = current_positions
joint_traj.header.stamp = rospy.Time.now()
self.core.pub_traj.publish(JointTrajectoryCommand("group", self.group_name, joint_traj))
rospy.sleep(moving_time + wp_moving_time)
self.T_sb = T_sd
self.joint_commands = joint_positions
self.set_trajectory_time(moving_time, accel_time)
return success
def ctrl_r(self, X_d):
# Get current joint positions
# joint_states are sent from Sawyer with names:
# [head_pan, right_j0, right_j1, right_j2, right_j3, right_j4,
# right_j5, right_j6, torso_t0]
# Before calculation, we need to remove the first and last elements
cur_theta_list = np.delete(self.cur_config, 0)
cur_theta_list = np.ndarray.tolist(np.delete(cur_theta_list, -1))
# Get desired end endeff transform
# EndEff frame initially in displacement-quaternion form...
p_d, Q_d = mr.TFtoMatrix(X_d)
# ...for ease of use with the Modern Robotics text, the frame is
# changed to transform matrix form using the tflistener library
X_d = self.tf_lis.fromTranslationRotation(p_d, Q_d)
# Current EndEff tranform is found using forward kinematics
X_cur = mr.FKinBody(self.M, self.B_list, cur_theta_list)
# EndEff transform error X_e is found from [X_e] = log(X^(-1)X_d)
X_e = mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(X_cur), X_d)))
# Find integral error
## Can induce instability, use with caution!
self.int_err = (self.int_err + X_e) * (0.5 / 20)
# Hard limit each int_err element to 0.5 to prevent runaway
for i in range(len(self.int_err)):
err_i = self.int_err[i]
if abs(err_i) > 0.5:
self.int_err[i] = np.sign(err_i) * 0.5
# Calculate body twist command from V_b = K_p . X_e + K_i . int(X_e)
V_b = np.dot(self.Kp, X_e) + np.dot(self.Ki, self.int_err)
# Calculate body Jacobian from robot_des and current joint positions
J_b = mr.JacobianBody(self.B_list, cur_theta_list)
# Calculate joint velocity command from theta_dot = pinv(J_b)V_b
J_b_pinv = np.linalg.pinv(J_b)
theta_dot = np.dot(J_b_pinv, V_b)
# Limit joint speed to 0.6 rad/sec
for i in range(len(theta_dot)):
if abs(theta_dot[i]) > 0.6:
theta_dot[i] = np.sign(theta_dot[i]) * 0.6
# Reinsert a 0 joint velocity command for the head_pan joint
theta_dot = np.insert(theta_dot, 1, 0)
# Convert to list because JointCommand messages are PICKY
self.joint_ctrl_msg.velocity = np.ndarray.tolist(theta_dot)
# Publish the JointCommand message
self.pub_joint_ctrl_msg()
def set_ee_cartesian_trajectory(self,
x=0,
y=0,
z=0,
roll=0,
pitch=0,
yaw=0,
moving_time=2.0,
wp_period=0.02):
rpy = ang.rotationMatrixToEulerAngles(self.T_sb[:3, :3])
T_sy = np.identity(4)
T_sy[:2, :2] = ang.yawToRotationMatrix(rpy[2])
T_yb = np.dot(mr.TransInv(T_sy), self.T_sb)
rpy = ang.rotationMatrixToEulerAngles(T_yb[:3, :3])
N = int(moving_time / wp_period)
inc = 1.0 / float(N)
joint_traj = []
joint_positions = list(self.joint_commands)
for i in range(N + 1):
joint_traj.append(joint_positions)
if (i == N):
break
T_yb[:3, 3] += [inc * x, inc * y, inc * z]
rpy[0] += inc * roll
rpy[1] += inc * pitch
rpy[2] += inc * yaw
T_yb[:3, :3] = ang.eulerAnglesToRotationMatrix(rpy)
T_sd = np.dot(T_sy, T_yb)
theta_list, success = self.set_ee_pose_matrix(
T_sd, joint_positions, False)
if success:
joint_positions = theta_list
else:
rospy.loginfo(
"%.1f%% of trajectory successfully planned. Trajectory will not be executed."
% (i / float(N) * 100))
break
if success:
mode = self.core.mode
if (mode != 1):
self.core.robot_smart_mode_reset(1)
r = rospy.Rate(1 / wp_period)
for cmd in joint_traj:
self.core.robot_move_servoj(cmd)
r.sleep()
self.T_sb = T_sd
self.joint_commands = joint_positions
return success
def p7():
print('P7')
Vs = np.array([3, 2, 1, -1, -2, -3])
Tas = np.array([[0, 0, 1, -1],
[-1, 0, 0, 0],
[0, -1, 0, 0],
[0, 0, 0, 1]])
Tsa = np.array([[0, -1, 0, 0],
[0, 0, -1, 0],
[1, 0, 0, 1],
[0, 0, 0, 1]])
assert(np.array_equal(Tas, mr.TransInv(Tsa)))
result = np.dot(mr.Adjoint(Tas), Vs)
print(result)
def IKinBodyIterates(Blist, M, T, thetalist0, eomg, ev):
thetalist = thetalist0
i = 0
Vb = mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(mr.FKinBody(M, Blist, \
thetalist)), T)))
err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \
or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev
max_i = 20
jointVecIter = thetalist
while (err and i < max_i):
thetalist = thetalist \
+ np.dot(np.linalg.pinv(mr.JacobianBody(Blist, \
thetalist)), Vb)
jointVecIter = np.vstack(
[jointVecIter,
thetalist]) #joining present joining angles to previous
i = i + 1
print("Iteration ", i, " :")
print("joint vector :\n", thetalist)
print("SE(3) end-effector config : \n",
mr.FKinBody(M, Blist, thetalist))
Vb \
= mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(mr.FKinBody(M, Blist, \
thetalist)), T)))
print("error twist V_b : \n", Vb)
err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \
or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev
print("angular error magnitude ||omega_b|| : ",
np.linalg.norm([Vb[0], Vb[1], Vb[2]]))
print("linear error magnitude ||v_b|| : ",
np.linalg.norm([Vb[3], Vb[4], Vb[5]]))
np.savetxt("iterator.csv", jointVecIter,
delimiter=",") # Saving to txt format
return (thetalist, not err)
def FeedbackControl(X, Xd, Xd_next, Kp, Ki, delta_t, J_arm, J_chassis):
"""
X_err_i: integral of the error
V: feed-forward + PI controller equation
Je: Jacobian of end-effector
command: end-effector inverse applied to PI equation
X_err: end-effector error
"""
global X_err_i
Vd = mr.se3ToVec(mr.MatrixLog6(np.matmul(mr.TransInv(Xd),
Xd_next))) / delta_t
X_err = mr.se3ToVec(mr.MatrixLog6(np.matmul(mr.TransInv(X), Xd)))
X_err_i = X_err_i + X_err * delta_t
a = mr.Adjoint(np.matmul(mr.TransInv(X), Xd)).dot(Vd)
b = Kp.dot(X_err)
c = Ki.dot(X_err_i)
V = a + b + c
Je = np.concatenate((J_chassis, J_arm), axis=1)
Je_inv = np.linalg.pinv(Je, 1e-2)
command = np.matmul(Je_inv, V)
return command, X_err
def convertPointBprimeFORIK(newP0bq,newP1bq,newP2bq,newP3bq,newP4bq,newP5bq): Tpinleg0 = np.dot(mrcl.TransInv(TbodyTOhip0),newP0bq) Tpinleg1 = np.dot(mrcl.TransInv(TbodyTOhip1),newP1bq) Tpinleg2 = np.dot(mrcl.TransInv(TbodyTOhip2),newP2bq) Tpinleg3 = np.dot(mrcl.TransInv(TbodyTOhip3),newP3bq) Tpinleg4 = np.dot(mrcl.TransInv(TbodyTOhip4),newP4bq) Tpinleg5 = np.dot(mrcl.TransInv(TbodyTOhip5),newP5bq) return Tpinleg0, Tpinleg1, Tpinleg2, Tpinleg3, Tpinleg4, Tpinleg5
def teleoperation_pose_cb(self, data):
self.is_master_pose_received = True
#print("position: {}".format(data.pose.position))
self.current_position = np.array(
[data.pose.position.x, data.pose.position.y, data.pose.position.z])
self.q_original = Quaternion(
w=data.pose.orientation.w,
x=data.pose.orientation.x,
y=data.pose.orientation.y,
z=data.pose.orientation.z,
)
master_pose_original = matrix_from_pose_msg(data.pose)
T_z90 = np.zeros((4, 4))
T_z90[0, 1] = 1
T_z90[1, 0] = -1
T_z90[2, 2] = 1
T_z90[3, 3] = 1
self.master_pose_original = np.matmul(mr.TransInv(T_z90),
master_pose_original)
def ctrl_r(self):
self.cur_theta_list = np.delete(self.main_js.position, 1)
self.X_d = self.tf_lis.fromTranslationRotation(self.p_d, self.Q_d)
self.X = mr.FKinBody(self.M, self.B_list, self.cur_theta_list)
self.X_e = mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(self.X), self.X_d)))
self.int_err = (self.int_err + self.X_e) * (0.5/20)
# hard limit int_err elements to 0.5 per to prevent int_err runaway
for i in range(len(self.int_err)):
err_i = self.int_err[i]
if abs(err_i) > 0.5:
self.int_err[i] = np.sign(err_i) * 0.5
self.V_b = np.dot(self.Kp, self.X_e) + np.dot(self.Ki, self.int_err)
self.J_b = mr.JacobianBody(self.B_list, self.cur_theta_list)
self.theta_dot = np.dot(np.linalg.pinv(self.J_b), self.V_b)
for i in range(len(self.theta_dot)):
if abs(self.theta_dot[i]) > 2:
self.theta_dot[i] = np.sign(self.theta_dot[i])*2
self.delt_theta = self.theta_dot*(1.0/20)
self.delt_theta = np.insert(self.delt_theta, 1, 0)
self.main_js.position += self.delt_theta
def get_jacobian(Baxis_mat, arm_joints_value, F_matrix, M0e, Tb0): Baxis_mat = np.array(Baxis_mat) arm_joints_value = np.array(arm_joints_value) # the F_matrix will be weighted by wheels_influence_factor # if the value if this factor is large, the pseudo inverse will output larger wheel speeds F_matrix = np.array(F_matrix)*(wheels_influence_factor) M0e = np.array(M0e) Tb0 = np.array(Tb0) T0e = mr.FKinBody(M0e, Baxis_mat, arm_joints_value) Teb = mr.TransInv(Tb0 @ T0e) # constituting base jacobian or F6(spatial) matrix relating the base wheel speed to the end-effector speed _ ,cols = F_matrix.shape z_vector = np.zeros((1, cols)) F_matrix = np.vstack((z_vector, z_vector, F_matrix, z_vector)) Base_Jacobian = mr.Adjoint(Teb) @ F_matrix # constituting the arm jacobian Arm_Jacobian = mr.JacobianBody(Baxis_mat, arm_joints_value) jacobian = np.hstack((Base_Jacobian, Arm_Jacobian)) return jacobian
def cal_fitness(population):
population_all = np.array(population).copy()
for i in range(population_all.shape[0]):
se3 = mr.MatrixLog6(np.dot(mr.TransInv(mr.FKinBody(M, Blist, \
population_all[i])), T))
Vb = mr.se3ToVec(se3)
ew = np.linalg.norm([Vb[0], Vb[1], Vb[2]])
el = np.linalg.norm([Vb[3], Vb[4], Vb[5]])
if i == 0:
fitness_norm = np.linalg.norm(Vb)
err = ew > eomg \
or el > ev
else:
fitness_norm = np.append(fitness_norm, np.linalg.norm(Vb))
err = np.append(err, (ew > eomg \
or el > ev))
fitness_array = fitness_norm.reshape(-1, 10)
return (fitness_array, err)
def inverse_kinematics_rcm(self, pose):
pose_r_jaw = np.matmul(mr.TransInv(self.T_s_r), pose)
# print("tar jaw in RCM frame: {}".format(pose_r_jaw))
# print("cur intra joint angle: {}".format(self.intra_joint_angle))
# print("fk for cur intra joint angle in RCM frame: {}".format(
# self.forward_kinematics(self.intra_joint_angle, 'jaw', 'r')))
tar_intra_joint_angle, err_intra_ik = self.inverse_kinematics(
pose_r_jaw, self.intra_joint_angle, frame='jaw', ref='r')
self.intra_joint_angle = tar_intra_joint_angle.copy()
# print("tar intra joint angle: {}".format(tar_intra_joint_angle))
# print("fk for tar intra joint angle in RCM frame: {}".format(self.forward_kinematics(tar_intra_joint_angle, 'jaw', 'r')))
# print("intra IK succ: {}".format(err_intra_ik))
# print("original intra end joint angle: {}".format(self.intra_end_joint_angle))
self.intra_end_joint_angle[0:4] = tar_intra_joint_angle[0:4]
# print("tar intra end joint angle: {}".format(self.intra_end_joint_angle))
tar_pose_r_e = self.forward_kinematics(self.intra_end_joint_angle,
frame='end',
ref='r')
tar_pose_s_e = np.matmul(self.T_s_r, tar_pose_r_e)
# print("tar_pose_s_e: {}".format(tar_pose_s_e))
tar_robot_joint_angle, _ = self.inverse_kinematics(
tar_pose_s_e,
self.joint_angle[:self.robot_dof],
frame='end',
ref='s')
# print("tar_robot_joint_angle: {}".format(tar_robot_joint_angle))
tar_joint_angle = self.joint_angle.copy()
tar_joint_angle[:self.robot_dof] = np.array(tar_robot_joint_angle)
tar_joint_angle[-2:] = tar_intra_joint_angle[-2:]
# print("tar_joint_angle: {}".format(tar_joint_angle))
# self.last_joint_angle = tar_joint_angle
# self.last_intra_joint_angle = tar_intra_joint_angle
# self.last_intra_end_joint_angle = tar_robot_joint_angle
return tar_joint_angle
def calc_joint_vel(self):
""" Deprecated version of joint vel command calculation (Only feed-forward)
"""
rospy.logdebug("Calculating joint velocities...")
# Body stuff
Tbs = self.M0 #
Blist = self.Blist #
# Current joint angles
self.get_q_now()
with self.mutex:
q_now = self.q #1x7 mx
# Desired config: base to desired - Tbd
with self.mutex:
T_sd = self.T_goal #
self.current_time = time.time()
delta_time = self.current_time - self.last_time
# Find transform from current pose to desired pose, error
# refer to CH04 >> the product of exponential formula for open-chain manipulator
# sequence:
# 1. FKinBody (M, Blist, thetalist)
# M: the home config. of the e.e.
# Blist: the joint screw axes in the ee frame, when the manipulator is @ the home pose
# thetalist : a list of current joints list
# We get the new transformation matrix T for newly given joint angles
#
# 1). np.array(Blist)[:,i] >> retrieve one axis' joint screw
# 2). ex) [s10, -c10, 0., -1.0155*c10, -1.0155*s10, -0.1603] -> S(theta)
# 3). _out = VecTose3(_in)
# # Takes a 6-vector (representing a spatial velocity).
# # Returns the corresponding 4x4 se(3) matrix.
# 4). _out = MatrixExp6(_in)
# # Takes a se(3) representation of exponential coordinate
# # Returns a T matrix SE(3) that is achieved by traveling along/about the
# # screw axis S for a distance theta from an initial configuration T = I(dentitiy)
# 5). np.dot (M (from base to robot's e.e. @home pose , and multiplying exp coord(6), we get new FK pose
#
# 2. TransInv >> we get the inverse of homogen TF matrix
# 3. error = np.dot (T_new, T_sd) >> TF matrix from cur pose to desired pose
# 4. Vb >>compute the desired body twist vector from the TF matrix
# 5. JacobianBody:
# # In: Blist, and q_now >> Out : T IN SE(3) representing the end-effector frame when the joints are
# at the specified coordinates
e = np.dot(r.TransInv(r.FKinBody(Tbs, Blist, q_now)),
T_sd) # Modern robotics pp 230 22Nff
# Desired TWIST: MatrixLog6 SE(3) -> se(3) exp coord
Vb = r.se3ToVec(r.MatrixLog6(e)) # shape : (6,)
# Construct BODY JACOBIAN for current config
Jb = r.JacobianBody(Blist, q_now) #6x7 mx # shape (6,7)
# WE NEED POSITION FEEDBACK CONTROLLER
# Desired ang vel - Eq 5 from Chiaverini & Siciliano, 1994
# Managing singularities: naive least-squares damping
n = Jb.shape[-1] #Size of last row, n = 7 joints
# OR WE CAN USE NUMPY' PINV METHOD
invterm = np.linalg.inv(
np.dot(Jb.T, Jb) + pow(self.damping, 2) * np.eye(n)) #
qdot_new = np.dot(
np.dot(invterm, Jb.T),
Vb) # It seems little bit akward...? >>Eq 6.7 on pp 233 of MR book
self.qdot = qdot_new #1x7
# Constructing dictionary
qdot_output = dict(zip(self.names, self.qdot))
# Setting Sawyer right arm joint velocities
self.limb.set_joint_velocities(qdot_output)
# print qdot_output
return