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 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 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 IKinBodyIterates(Blist, M, T, thetas_guess, eomg, ev, max_iteration=20):
thetas = np.array(thetas_guess).copy().astype(np.float)
Tsd = np.array(T).copy().astype(np.float)
success = False
guesses = [thetas_guess]
print("Initial Guess: {0}".format(thetas))
for i in range(max_iteration):
print("Iteration {0}\n".format(i))
# Calculate space frame transform from end effector to desired position.
Tsb = mr.FKinBody(M, Blist, thetas)
print("joint vector: {0}".format(thetas))
print("SE3 end effector config: \n{0}".format(Tsb))
Tbd = np.linalg.inv(Tsb) @ Tsd
# Twist in matrix form.
Vb_se3 = mr.MatrixLog6(Tbd)
Vb = mr.se3ToVec(Vb_se3)
print("error twist: {0}".format(Vb))
print("angular error magitude || omega_b ||: {0}".format(
np.linalg.norm(Vb[0:3])))
print("linear error magnitude || v_b ||: {0}".format(
np.linalg.norm(Vb[3:6])))
print("-------------------\n")
# Check if result is a success.
if np.linalg.norm(Vb[0:3]) < eomg and np.linalg.norm(Vb[3:6]) < ev:
success = True
break
Jac = mr.JacobianBody(Blist, thetas)
thetas = (thetas + np.linalg.pinv(Jac) @ Vb) % (2 * np.pi)
guesses.append(thetas)
return (thetas, success, np.array(guesses))
def __init__(self):
rospy.loginfo("Creating VelocityController class")
# Grab M0 and Blist from saywer_MR_description.py
self.M0 = s.M #Zero config of right_hand
self.Blist = s.Blist #6x7 screw axes mx of right arm
# Shared variables
self.mutex = threading.Lock()
self.time_limit = rospy.get_param("~time_limit", TIME_LIMIT)
self.damping = rospy.get_param("~damping", DAMPING)
self.joint_vel_limit = rospy.get_param("~joint_vel_limit", JOINT_VEL_LIMIT)
self.q = np.zeros(4) # Joint angles
self.qdot = np.zeros(4) # Joint velocities
self.T_goal = r.FKinBody(self.M0, self.Blist, self.q[0:3])
self.isCalcOK = False
# Subscriber
self.joint_states_sub = rospy.Subscriber('/open_manipulator/joint_states', JointState, self.joint_states_cb)
self.ref_pose_sub = rospy.Subscriber('/teacher/ik_vel/', Pose, self.ref_pose_cb)
# Command publisher
self.j1_pos_command_pub = rospy.Publisher('/open_manipulator/joint1_position/command', Float64, queue_size=3)
self.j2_pos_command_pub = rospy.Publisher('/open_manipulator/joint2_position/command', Float64, queue_size=3)
self.j3_pos_command_pub = rospy.Publisher('/open_manipulator/joint3_position/command', Float64, queue_size=3)
self.j4_pos_command_pub = rospy.Publisher('/open_manipulator/joint4_position/command', Float64, queue_size=3)
self.joint_pos_command_to_dxl_pub = rospy.Publisher('/open_manipulator/joint_position/command', Float64MultiArray, queue_size=3)
self.r = rospy.Rate(100)
while not rospy.is_shutdown():
self.calc_joint_vel()
self.r.sleep()
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 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 jacobsLadder(current_config):
"""
Parameters
----------
phi, x, y : robot chassis configuration
j1, j2, j3, j4, j5 arm configuration
w1, w2, w3, w4 wheel speed configuration
gripper_state : 0=open; 1=closed.
Returns
-------
x : Forward kinematics
Ja, Jb : Jacobian matricies
"""
phi, x, y, j1, j2, j3, j4, j5, w1, w2, w3, w4, g = current_config
thetalist = np.array([j1, j2, j3, j4, j5])
# Tsb = np.array([[]])
Tsb = np.array([[np.cos(phi), -np.sin(phi), 0, x],
[np.sin(phi), np.cos(phi), 0, y], [0, 0, 1, 0.0963],
[0, 0, 0, 1]])
Tb0 = np.array([[1, 0, 0, 0.1662], [0, 1, 0, 0], [0, 0, 1, 0.0026],
[0, 0, 0, 1]])
Ja = mr.JacobianBody(Blist, thetalist)
T0e = mr.FKinBody(M, Blist, thetalist)
X = np.dot(np.dot(Tsb, Tb0), T0e)
Jb = np.dot(mr.Adjoint(np.dot(la.inv(T0e), Tb0)), H)
return X, Ja, Jb
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 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 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 ref_pose_cb(self, some_pose): # Takes target pose, returns ref se3
p = np.array([some_pose.position.x, some_pose.position.y, some_pose.position.z])
quat = np.array([some_pose.orientation.x, some_pose.orientation.y, some_pose.orientation.z, some_pose.orientation.w])
goal_tmp = tr.compose_matrix(angles=tr.euler_from_quaternion(quat, 'sxyz'), translate=p)
with self.mutex:
goal_tmp = r.FKinBody(self.M0, self.Blist, np.array([0.0, 0.0, 0.0, 1.57]))
goal_tmp[0:3,3] = p
with self.mutex:
self.T_goal = goal_tmp
def youBot_Tbe(thetaList):
"""Computes the transform of youBot End effector in robot base frame
:param configuration: A 5-vector representing the current configuration of the robot arm.
5 variables for the arm configuration (J1, J2, J3, J4, J5),
:return: Transform of End effector in base frame
"""
T0e = mr.FKinBody(M, Blist, thetaList)
Tbe = np.dot(Tb0, T0e)
return Tbe
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 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 find_X(self, configuration): #X is equal to Tse
#input - configuration (phi, x, y, theta1, theta2, theta3, theta4, theta5)
phi = configuration[0]
x = configuration[1]
y = configuration[2]
Tsb = KukaYoubot.calc_Tsb(phi, x, y)
thetalist = np.array([
configuration[3], configuration[4], configuration[5],
configuration[6], configuration[7]
])
T0e = mr.FKinBody(KukaYoubot.M0e, KukaYoubot.Blist, thetalist)
Ts0 = np.matmul(Tsb, KukaYoubot.Tb0)
Tse = np.round(np.matmul(Ts0, T0e), 5)
return Tse
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 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 Convert(phi,x,y,j1,j2,j3,j4,j5): """ Given the current configurations of robot chassis and joints, return the Jacobian matrices. """ thetalist = np.array([j1,j2,j3,j4,j5]) Tsb = np.array([[cos(phi), -sin(phi), 0, x],[sin(phi), cos(phi), 0, y],[0, 0, 1, 0.0963],[0, 0, 0, 1]]) Tb0 = np.array([[1, 0, 0, 0.1662],[0, 1, 0, 0],[0, 0, 1, 0.0026],[0, 0, 0, 1]]) Ja = mr.JacobianBody(B,thetalist) T0e = mr.FKinBody(M0e,B,thetalist) X = np.dot(np.dot(Tsb,Tb0),T0e) Jb = np.dot(mr.Adjoint(np.dot(np.linalg.inv(T0e),Tb0)), F6) return X,Ja,Jb
def calc_joint_vel(self):
rospy.logdebug("Calculating joint velocities...")
# Body stuff
Tbs = self.M0
Blist = self.Blist
# Desired config: base to desired - Tbd
with self.mutex:
T_sd = self.T_goal
q_now = self.q
T_cur = r.FKinBody(Tbs, Blist, q_now)
e = np.zeros(6)
e[0:3] = self.get_phi(T_sd[0:3,0:3],T_cur[0:3,0:3])
e[3:6] = T_sd[0:3,3] - T_cur[0:3,3]
# Construct BODY JACOBIAN for current config
Jb = r.JacobianBody(Blist, q_now) #6x7 mx
Jv = Jb[3:6,:]
# Desired ang vel - Eq 5 from Chiaverini & Siciliano, 1994
# Managing singularities: naive least-squares damping
invterm = np.linalg.inv(np.dot(Jv,Jv.T)+ pow(self.damping, 2)*np.eye(3))
qdot_new = np.dot(np.dot(Jv.T, invterm),e[3:6])
# 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 = 2.0*(qdot_new/scale)*self.joint_vel_limit
self.qdot = qdot_new #1x7
dt = 0.01
q_desired = q_now + qdot_new * dt
self.j1_pos_command_pub.publish(q_desired[0])
self.j2_pos_command_pub.publish(q_desired[1])
self.j3_pos_command_pub.publish(q_desired[2])
self.j4_pos_command_pub.publish(q_desired[3])
self.joint_pos_command_to_dxl_pub.publish(data=q_desired)
return
def youBot_selfCollisionCheck(thetaList):
"""Checks if the provided robot arm configuration is in self collision with youBot
:param configuration: A 5-vector representing the current configuration of the robot arm
5 variables for the arm configuration (J1, J2, J3, J4, J5),
:return: A boolean value, status
True - No self collision.
False - Self collision
This funtion checks whether the end effector of the youBot arm is not in restricted / self colliding
areas. The restricted area is designed such that, if the end effector position is in a non restricted area,
the arm will not be in self collision with the robot or the arm links themselves.
The restricted area is modeled as a combination of a cuboid (representing robot base)
and a cylinder (representing arm base)
"""
noCollision = True
T0e = mr.FKinBody(M, Blist, thetaList)
r, [x, y, z] = mr.TransToRp(T0e)
# T0e position must not be in the cylinder region of the robot arm base.
# Cylinder region radius : 0.1562m. Height 0.3604m
min_radius = 0.1562
min_height = 0.3604
radial_distance = math.sqrt(math.pow(x, 2) + math.pow(y, 2))
if (radial_distance < min_radius) and (z < min_height):
noCollision = False
return noCollision
Tbe = np.dot(Tb0, T0e)
r, [x, y, z] = mr.TransToRp(T0e)
# Tbe position must not be in the cuboid region of the youBot base
# Cuboid region coordinates:
x_min = -0.35
x_max = +0.35
y_min = -0.336
y_max = +0.336
z_min = 0
z_max = 0.175
if ((x_min < x) and
(x < x_max)) and ((y_min < y) and
(y < y_max)) and ((z_min < z and z < z_max)):
noCollision = False
return noCollision
return noCollision
def Fkin(config, Baxis_mat, M0e, Tb0): # calculate forward kinematics of Kuka youbot Baxis_mat = np.array(Baxis_mat) config = np.array(config) M0e = np.array(M0e) Tb0 = np.array(Tb0) # Tsb the description of the chassis frame with respect to the spatial frame s chassis_theta ,chassis_x, chassis_y = config[0], config[1], config[2] cos_th = np.cos(chassis_theta) sin_th = np.sin(chassis_theta) Tsb = [[cos_th, -sin_th, 0, chassis_x], [sin_th, cos_th, 0, chassis_y], [0, 0, 1, 0.0963],[0, 0, 0, 1]] # T0e is the forward kinematics of the arm, which gives ends-effector description wrt the base of the arm frame 0 arm_joints_value = config[3:8] T0e = mr.FKinBody(M0e, Baxis_mat, arm_joints_value) Tse = Tsb @ Tb0 @ T0e return Tse
def FeedbackControl(X, XD, XDN, KP, KI, dt):
XDI = np.linalg.inv(XD)
XDDN = np.dot(XDI, XDN)
se3VD = (1 / dt) * mr.MatrixLog6(
XDDN) # Calculating Feedforward Twist in desired frame
VD = mr.se3ToVec(se3VD)
XI = np.linalg.inv(X)
XIXD = np.dot(
XI, XD
) # Calculating Adjoint for converting twist in desired frame to end effector frame
A = mr.Adjoint(XIXD)
se3XE = mr.MatrixLog6(XIXD)
XE = mr.se3ToVec(se3XE) # Error twist
I = dt * XE
V = np.dot(A, VD) + np.dot(KP, XE) + np.dot(
KI, I) # Twist in end effector frame using PID controller
#V=np.dot(KP,XE) + np.dot(KI,I)
T0E = mr.FKinBody(M, Blist, thetalist)
T0EI = np.linalg.inv(T0E)
TB0I = np.linalg.inv(TB0)
TEB = np.dot(T0EI, TB0I)
C = mr.Adjoint(TEB)
FM = 0.011875 * np.array([
[-2.5974, 2.5974, 2.5974, -2.5974],
[1, 1, 1, 1],
[-1, 1, -1, 1],
])
F6 = np.zeros((6, 4))
F6[2:5, :] = FM
JB = np.dot(C, F6) # Calculating Base Jacobian
JA = mr.JacobianBody(Blist, thetalist) # Calculating Arm Jacobian
JE = np.zeros((6, 9))
JE[:, 0:5] = JA
JE[:, 5:9] = JB
JEI = np.linalg.pinv(JE)
Matrix = np.dot(JEI, V) # Calculating speed and angular velocity
return Matrix
def getActConfig(curConfig):
l = 0.47 / 2
w = 0.3 / 2
r = 0.0475
theta_cur = np.array(curConfig[3:8])
phi, x, y = np.array(curConfig[0:3])
F = (r / 4) * np.array(
[[-1 / (l + w), 1 / (l + w), 1 / (l + w), -1 /
(l + w)], [1, 1, 1, 1], [-1, 1, -1, 1]])
Blist, M, Tb0 = getConsts()
T0e = mr.FKinBody(M, Blist, theta_cur)
Tsb = np.array([[m.cos(phi),-m.sin(phi),0,x],\
[m.sin(phi),m.cos(phi),0,y],\
[0,0,1,0.0963],\
[0,0,0,1]])
X = np.dot(Tsb, np.dot(Tb0, T0e))
return X, theta_cur, F
def getPsuedo(F, theta_cur):
Blist, M, Tb0 = getConsts()
T0e = mr.FKinBody(M, Blist, theta_cur)
Ja = mr.JacobianBody(Blist, theta_cur)
F6 = np.vstack([
np.zeros(len(F[0])),
np.zeros(len(F[0])), F[0], F[1], F[2],
np.zeros(len(F[0]))
])
Jb = np.dot(np.dot(mr.Adjoint(np.linalg.inv(T0e)), \
mr.Adjoint(np.linalg.inv(Tb0))),\
F6)
Je = np.c_[Jb, Ja]
pJe = np.linalg.pinv(Je, 0.001)
# print('Je:',Je)
# print('theta:',theta_cur)
return pJe, Ja, Jb
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 __init__(self):
self.angles = [0,0,pi/2,-pi/2,pi/2,pi/2,pi/2+sin(radians(10)) ]
self.vely = 0
self.ik_begin = False
self.goal = mr.FKinBody(smr.M,smr.Blist,self.angles)
self.goal[0:3,0:3] = np.array([[0,-1,0],[1,0,0],[0,0,1]])
self.js = []
self.limb = intera_interface.Limb("right")
self.start_angles = {"right_j0":self.angles[0], "right_j1":self.angles[1], "right_j2":self.angles[2],
"right_j3": self.angles[3], "right_j4":self.angles[4], "right_j5":self.angles[5], "right_j6":self.angles[6]}
self.limb.move_to_joint_positions(self.start_angles)
self.mutex = threading.Lock()
self.yp = rospy.Subscriber("yPoint", Point, self.Pnt)
self.jss = rospy.Subscriber("/robot/joint_states", JointState,self.get_js)
self.tim = rospy.Timer(rospy.Duration(1/100.), self.step_ik)
self.key = rospy.Timer(rospy.Duration(1/10.), self.key_press)
self.status = rospy.Publisher("reset_control",String, queue_size=3)
self.xdot = 0
self.count = 0
self.slow = True
self.xPos = -.2075
def jacobianify(chassis_phi, chassis_x, chassis_y, J1, J2, J3, J4, J5):
"""
Converts the robot configuration into Jacobian matrices
chassis_phi ... J5: robot configuration
X: current actual end-effector configuration
J_arm: Jacobian of arm
J_chassis: Jacobian of chassis/base
"""
arm_config = np.array([J1, J2, J3, J4, J5])
T0e = mr.FKinBody(M0e, screw, arm_config)
Tsb = np.array([[np.cos(chassis_phi), -np.sin(chassis_phi), 0, chassis_x],
[np.sin(chassis_phi),
np.cos(chassis_phi), 0, chassis_y], [0, 0, 1, 0.0963],
[0, 0, 0, 1]])
Ts0 = np.matmul(Tsb, Tb0) #np.dot(Tsb, Tb0)
X = np.matmul(Ts0, T0e) #np.dot(Ts0, T0e)
J_arm = mr.JacobianBody(screw, arm_config)
#Jc_pre = np.linalg.inv(T0e),Tb0
#J_chassis = np.dot(mr.Adjoint(np.dot(np.linalg.inv(T0e),Tb0)), config_6)
J_chassis = np.matmul(mr.Adjoint(np.linalg.inv(np.matmul(Tb0, T0e))),
config_6)
return X, J_arm, J_chassis
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