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 feedForwardControl(X, Xd, XdNext, currentState, controls, Kp, Ki):
"""
sets the controVector to be passed on to the next state function.
"""
T_eb = np.dot(np.linalg.inv(endEffectorinSpace(currentState)),
basePosition(currentState.chasisState)) ###doubt
Jbase = np.dot(mr.Adjoint(T_eb), youBotProperties.F6)
Jarm = mr.JacobianBody(youBotProperties.Blist, currentState.jointState)
Je = np.concatenate((Jbase, Jarm), axis=1)
# Je = np.concatenate((Jarm,Jbase),axis=1)
invJe = np.linalg.pinv(Je)
VdBracket = (1 / youBotProperties.deltaT) * mr.MatrixLog6(
np.dot(np.linalg.inv(Xd), XdNext))
Vd = mr.se3ToVec(VdBracket)
#print("Vd",Vd)
Xerr = mr.se3ToVec(mr.MatrixLog6(np.dot(np.linalg.inv(X), Xd)))
youBotProperties.ErrorInt = youBotProperties.ErrorInt + Xerr * youBotProperties.deltaT
print(Xerr)
feedForward = np.dot(mr.Adjoint(np.dot(np.linalg.inv(X), Xd)), Vd)
#print("FeedForward:",feedForward)
V = feedForward + np.dot(Kp, Xerr) * np.dot(Ki, youBotProperties.ErrorInt)
u_thetadot = np.dot(invJe, V)
controls.wheelSpeeds = u_thetadot[0:4]
controls.jointSpeeds = u_thetadot[4:9]
return Xerr
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 get_V_command(self, i):
'''
Returns the feedforward + feedback body twist V as the control command for the robot.
Input: index of the current trajectory point in the trajectory list
Output: Body Feedforwad Twist V that takes the robot from the current pose to the next desired pose.
'''
#feedforward
self.current_X, self.Teb = get_poses(self.pose_8_list)
Xd = to_X(self.trajectory_list[i])
Xd_next = to_X(self.trajectory_list[i + 1])
self.gripper_state = self.trajectory_list[i][-1]
X_d2dnext = np.linalg.inv(Xd).dot(Xd_next)
X_err = np.linalg.inv(self.current_X).dot(
Xd) #current_x is the end effector pose in the world frame
Adj = mr.Adjoint(X_err)
V_feedforward = 1.0 / DELTA_T * Adj.dot(
(mr.se3ToVec(mr.MatrixLog6(X_d2dnext))))
#feedforward + feedback
X_err_vec = mr.se3ToVec(mr.MatrixLog6(X_err))
self.total_error_vec_list = np.vstack(
(self.total_error_vec_list, X_err_vec)) #add to total error list
if self.mode == 'overshoot':
Kp = 1.0*np.array([[30.0, 0.0, 0.0, 0.0, 0.0, 0.0],\
[0.0, 20.0, 0.0, 0.0, 0.0, 0.0],\
[0.0, 0.0, 30.0, 0.0, 0.0, 0.0],\
[0.0, 0.0, 0.0, 200.0, 0.0, 0.0],\
[0.0, 0.0, 0.0, 0.0, 200.0, 0.0],\
[0.0, 0.0, 0.0, 0.0, 0.0, 30.0]])
else:
Kp = 1.0*np.array([[30.0, 0.0, 0.0, 0.0, 0.0, 0.0],\
[0.0, 20.0, 0.0, 0.0, 0.0, 0.0],\
[0.0, 0.0, 30.0, 0.0, 0.0, 0.0],\
[0.0, 0.0, 0.0, 20.0, 0.0, 0.0],\
[0.0, 0.0, 0.0, 0.0, 30.0, 0.0],\
[0.0, 0.0, 0.0, 0.0, 0.0, 30.0]])
Ki = 1.0*np.array([[0.1, 0.0, 0.0, 0.0, 0.0, 0.0],\
[0.0, 0.2, 0.0, 0.0, 0.0, 0.0],\
[0.0, 0.0, 0.2, 0.0, 0.0, 0.0],\
[0.0, 0.0, 0.0, 0.1, 0.0, 0.0],\
[0.0, 0.0, 0.0, 0.0, 0.1, 0.0],\
[0.0, 0.0, 0.0, 0.0, 0.0, 0.2]])
self.X_err_vec_integral += DELTA_T * X_err_vec
V = V_feedforward + Kp.dot(X_err_vec) + Ki.dot(self.X_err_vec_integral)
#V = Kp.dot( X_err_vec )
return V
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 FeedbackControl(X, Xd, Xdn, Kp, Ki, del_t, Xerr_int):
Xerr = mr.se3ToVec(mr.MatrixLog6(np.dot(np.linalg.inv(X), Xd)))
Xerr_int += Xerr * del_t
Vd = mr.se3ToVec(
(1 / del_t) * mr.MatrixLog6(np.dot(np.linalg.inv(Xd), Xdn)))
Adxxd = np.dot(mr.Adjoint(np.linalg.inv(X)), mr.Adjoint(Xd))
V = np.dot(Adxxd, Vd) + np.dot(Kp, Xerr) + np.dot(Ki, Xerr_int)
# print('Vd:',Vd)
# print('Adxxd.Vd:',np.dot(Adxxd,Vd))
# print('Xerr:',Xerr)
# print('V:',V)
return V, Xerr, Xerr_int
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 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 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 p15():
print('P15')
T = np.array([[0, -1, 0, 3],
[1, 0, 0, 0],
[0, 0, 1, 1],
[0, 0, 0, 1]])
result = mr.MatrixLog6(T)
print(result)
def FeedbackControl(X, Xd, Xd_next, gains, timeStep):
"""Calculates the kinematic task-space feedforward plus feedback control law
:param Tse: The current actual end-effector configuration X (also written Tse)
:param Tse_d: The current end-effector reference configuration Xd (i.e., Tse,d)
:param Tse_dNext: The end-effector reference configuration at the next timestep in the reference trajectory,
Xd,next (i.e., Tse,d,next), at a time delta_t later
:param gains: The PI gain matrices Kp and Ki
:param timeStep: The timestep delta_t between reference trajectory configurations
:return: The commanded end-effector twist V expressed in the end-effector frame {e}
and Xerr
Calculates the kinematic task-space feedforward plus feedback control law,
written both as Equation (11.16) and (13.37) in the textbook
"""
global XerrSum
Terr = np.dot(np.linalg.inv(X), Xd)
Xerr_se3 = mr.MatrixLog6(Terr)
Xerr = mr.se3ToVec(Xerr_se3)
Ad_Terr = mr.Adjoint(Terr)
# Updating integral
XerrSum = XerrSum + (Xerr * timeStep)
T_dNext = np.dot( np.linalg.inv(Xd), Xd_next)
Vd_se3 = mr.MatrixLog6(T_dNext)
# Time scaling Vd_se3
Vd_se3 = Vd_se3 / timeStep
Vd = mr.se3ToVec(Vd_se3)
# Generating gain matrices
kp = gains[0]
ki = gains[1]
for x in range(6):
Kp[x][x] = kp
Ki[x][x] = ki
# Control law
V = np.dot(Ad_Terr, Vd) + np.dot(Kp, Xerr) + np.dot(Ki, XerrSum)
return V, Xerr
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 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):
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 FeedbackControl(Xd,Xd_next,X,Kp,Ki,dt,Ja,Jb):
"""
Xd: Current end-effector configuration
Xd_next: End-effector configuration at next timestamp
X: Current actual end-effector configuration(Tse)
Kp, Ki: PI controller gains
Ja, Jb: Jacobian matrices of robot arm and robot chassis
"""
global Integral
"""
V(t) = [Ad_X^(-1)*Xd]*Vd(t) + Kp*Xerr(t) + Ki*Integral[Xerr(t)]dt
"""
Vd = mr.se3ToVec(mr.MatrixLog6(np.dot(np.linalg.inv(Xd),Xd_next))/dt)
term_1 = np.dot(mr.Adjoint(np.dot(np.linalg.inv(X),Xd)),Vd)
Xerr = mr.se3ToVec(mr.MatrixLog6(np.dot(np.linalg.inv(X),Xd)))
term_2 = np.dot(Kp,Xerr)
Integral = Integral + Xerr*dt
term_3 = np.dot(Ki,Integral)
Vt = term_1 + term_2 + term_3
Je = np.append(Jb,Ja,axis=1)
v = np.dot(np.linalg.pinv(Je,1e-4), Vt)
return v,Xerr
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 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 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 FeedbackControl(X, Xd, Xd_next, Kp, Ki, dt, accumulated_error):
iX_Xd = np.matmul(np.linalg.inv(X), Xd)
Adj_iX_Xd = mr.Adjoint(iX_Xd)
Vd = mr.se3ToVec(1/dt*(np.matmul(np.linalg.inv(Xd),Xd_next)))
Xerr = mr.se3ToVec(mr.MatrixLog6((np.matmul(np.linalg.inv(X),Xd))))
#print(Xerr)
accumulated_error = Xerr+accumulated_error
#print(accumulated_error)
feedback_error = np.matmul(Kp,Xerr)+np.matmul(Ki, accumulated_error*dt)
V = np.matmul(Adj_iX_Xd, Vd)+feedback_error
return V, Xerr
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 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 p2p_cart_cubic_screw(robot, T_end, ts):
T_start = robot.get_homogeneous()
a2 = 3 / (ts**2)
a3 = -2 / (ts**3)
t = 0
while t < ts:
t += robot.dt
s = a2 * t**2 + a3 * t**3
T = T_start @ mr.MatrixExp6(
mr.MatrixLog6(np.linalg.inv(T_start) @ T_end) * s)
pos = T[0:3, 3]
mat_rot = T[0:3, 0:3]
eul = rot2eul(mat_rot)
quat = p.getQuaternionFromEuler(eul)
joints = p.calculateInverseKinematics(
robot.robot_id,
endEffectorLinkIndex=robot.eef_link_idx,
targetPosition=pos,
targetOrientation=quat)
yield joints
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
def feedbackControl(config, Xd, Xd_next, Kp, Ki, dt, jointlimit):
# here we compute X
M = np.array([[1, 0, 0, 0.033], [0, 1, 0, 0], [0, 0, 1, 0.6546],
[0, 0, 0, 1]])
Blist = np.array([[0, 0, 1, 0, 0.033, 0], [0, -1, 0, -0.5076, 0, 0],
[0, -1, 0, -0.3526, 0, 0], [0, -1, 0, -0.2176, 0, 0],
[0, 0, 1, 0, 0, 0]]).T
Tb0 = np.array([[1, 0, 0, 0.1662], [0, 1, 0, 0], [0, 0, 1, 0.0026],
[0, 0, 0, 1]])
thetalist_initial = np.array(
[config[0, 3], config[0, 4], config[0, 5], config[0, 6], config[0, 7]])
T_sb_initial = np.array(
[[np.cos(config[0, 0]), -np.sin(config[0, 0]), 0, config[0, 1]],
[np.sin(config[0, 0]),
np.cos(config[0, 0]), 0, config[0, 2]], [0, 0, 1, 0.0963],
[0, 0, 0, 1]])
X = T_sb_initial.dot(Tb0).dot(mr.FKinBody(M, Blist, thetalist_initial))
# here we write down Vd
Vd = mr.se3ToVec((1 / dt) * mr.MatrixLog6(np.linalg.inv(Xd).dot(Xd_next)))
# here we write down Vb = Ad*Vd
Vb = mr.Adjoint(np.linalg.inv(X).dot(Xd)).dot(Vd)
# here we write down X_err
X_err = mr.se3ToVec(mr.MatrixLog6(np.linalg.inv(X).dot(Xd)))
# here we write down commanded twist
V = Vb + Kp * X_err + Ki * (X_err + X_err * dt)
# here we compute J_e
# first we write down J_arm
Blist = np.array([[0, 0, 1, 0, 0.033, 0], [0, -1, 0, -0.5076, 0, 0],
[0, -1, 0, -0.3526, 0, 0], [0, -1, 0, -0.2176, 0, 0],
[0, 0, 1, 0, 0, 0]]).T
thetalist = np.array(
[config[0, 3], config[0, 4], config[0, 5], config[0, 6], config[0, 7]])
J_arm = mr.JacobianBody(Blist, thetalist)
# second we write down J_base
r = 0.0475
l = 0.47 / 2
w = 0.3 / 2
gamma1 = -np.pi / 4
gamma2 = np.pi / 4
gamma3 = -np.pi / 4
gamma4 = np.pi / 4
beta = 0
x1 = l
y1 = w
x2 = l
y2 = -w
x3 = -l
y3 = -w
x4 = -l
y4 = w
# here we write down F6
a1 = np.array([1, np.tan(gamma1)])
a2 = np.array([1, np.tan(gamma2)])
a3 = np.array([1, np.tan(gamma3)])
a4 = np.array([1, np.tan(gamma4)])
b = np.array([[np.cos(beta), np.sin(beta)], [-np.sin(beta), np.cos(beta)]])
c1 = np.array([[-y1, 1, 0], [x1, 0, 1]])
c2 = np.array([[-y2, 1, 0], [x2, 0, 1]])
c3 = np.array([[-y3, 1, 0], [x3, 0, 1]])
c4 = np.array([[-y4, 1, 0], [x4, 0, 1]])
h1 = (((1 / r) * a1).dot(b)).dot(c1)
h2 = (((1 / r) * a2).dot(b)).dot(c2)
h3 = (((1 / r) * a3).dot(b)).dot(c3)
h4 = (((1 / r) * a4).dot(b)).dot(c4)
H0 = np.vstack((h1, h2, h3, h4))
F = np.linalg.pinv(H0)
F6 = np.array([[0, 0, 0, 0], [0, 0, 0, 0],
[F[0, 0], F[0, 1], F[0, 2], F[0, 3]],
[F[1, 0], F[1, 1], F[1, 2], F[1, 3]],
[F[2, 0], F[2, 1], F[2, 2], F[2, 3]], [0, 0, 0, 0]])
# then write down J_base
T_sb = np.array(
[[np.cos(config[0, 0]), -np.sin(config[0, 0]), 0, config[0, 1]],
[np.sin(config[0, 0]),
np.cos(config[0, 0]), 0, config[0, 2]], [0, 0, 1, 0.0963],
[0, 0, 0, 1]])
T_eb = np.linalg.inv(X).dot(T_sb)
J_base = mr.Adjoint(T_eb).dot(F6)
# here we test joint limits
if jointlimit == 1:
jointLimits(config, J_arm)
# finally we write down J_e
J_e = np.hstack((J_base, J_arm))
# here we write down speeds (u,thetadot)
speeds = np.linalg.pinv(J_e, rcond=1e-2).dot(V)
return V, speeds, X_err
