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 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 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 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 youBot_Je(configuration):
"""Computes the jacobian of youBot End effector given a robot configuration
:param configuration: A 12-vector representing the current configuration of the robot.
3 variables for the chassis configuration (theta, x, y),
5 variables for the arm configuration (J1, J2, J3, J4, J5),
and 4 variables for the wheel angles (W1, W2, W3, W4)
:return: Robot jacobian [Jbase, Jb_arm]
"""
# Generating Arm body jacobian
thetaList = configuration[3:8]
Jarm = mr.JacobianBody(Blist, thetaList)
# Generating Jbase
F3 = np.linalg.pinv(H)
F6 = np.zeros((6, 4))
F6[2:5, :] = F3
Tbe = youBot_Tbe(thetaList)
Teb = np.linalg.inv(Tbe)
Ad_Teb = mr.Adjoint(Teb)
Jbase = np.dot(Ad_Teb, F6)
Je = np.concatenate((Jbase, Jarm), axis=1)
return Je
def __init__(self, limb, tf_listener):
self._limb = limb
self._tf_listener = tf_listener
# h: hand, c: camera
try:
self._tf_listener.waitForTransform(
'/' + self._limb + '_hand', '/' + self._limb + '_hand_camera',
rospy.Time(), rospy.Duration(4.0))
(self._t_hc, self._R_hc) = self._tf_listener.lookupTransform(
'/' + self._limb + '_hand', '/' + self._limb + '_hand_camera',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print 'Error! Cannot find [{}_hand_camera] to [{}_hand] tf.'.format(
self._limb, self._limb)
sys.exit(0)
self._R_hc = quaternion_matrix(self._R_hc)
# frame transforms from camera to hand, only look up once
self._T_hc = modern_robotics.RpToTrans(self._R_hc[:3, :3], self._t_hc)
self._Ad_hc = modern_robotics.Adjoint(self._T_hc)
# frame transforms from hand to body, look up in every loop
self._T_bh = np.zeros((4, 4))
self._Ad_bh = np.zeros((6, 6))
self._arm = baxter_interface.limb.Limb(self._limb)
self._kin = baxter_kinematics(self._limb)
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 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 p10():
print('P10')
Fb = np.array([1, 0, 0, 2, 1, 0])
Tbs = np.array([[1, 0, 0, 0],
[0, 0, -1, 0],
[0, 1, 0, -2],
[0, 0, 0, 1]])
Fs = np.dot(np.transpose(mr.Adjoint(Tbs)), Fb)
print(Fs)
def __init__(self):
self.Blist = sw.Blist
self.M = sw.M
self.Slist = mr.Adjoint(self.M).dot(self.Blist)
self.eomg = 0.01 # positive tolerance on end-effector orientation error
self.ev = 0.001 # positive tolerance on end-effector position error
self.arm = None
self.listener = None
self.home_config = {
'right_j6': -1.3186796875,
'right_j5': 0.5414912109375,
'right_j4': 2.9682451171875,
'right_j3': 1.7662939453125,
'right_j2': -3.0350302734375,
'right_j1': 1.1202939453125,
'right_j0': -0.0001572265625
}
self.tracker_position = None
self.tracker_pos_mag = None
self.arm_position = None
self.sword_position = None
self.attack_position = None
self.tag_position = None
self.disp_mag = None
self.attack_disp_mag = None
self.fence_region = 1.50
self.fence_speed = 0.50
self.sword_zoffset = 0.25
self.end_effector_directions = [-1.0, 1.0]
self.tracker_twist = 50
self.end_effector_vel = np.zeros(6)
# velocities need to be passed in as a dictionary
self.joint_vels_dict = {}
# set the end effector twist
self.sword_twist = Twist()
self.arm_twist = Twist()
self.no_twist = Twist()
self.no_twist.linear.x = 0
self.no_twist.linear.y = 0
self.no_twist.linear.z = 0
self.no_twist.angular.x = 0
self.no_twist.angular.y = 0
self.no_twist.angular.z = 0
self.arm = Limb()
print("going to home configuration")
self.arm.set_joint_positions(self.home_config)
self.clash_pub = rospy.Publisher('clash', Float64, queue_size=1)
self.twist_sub = rospy.Subscriber('/vive/twist1',
TwistStamped,
self.twist_callback,
queue_size=1)
self.tf_listener = tf.TransformListener()
self.rate = rospy.Rate(10.0)
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 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 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 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 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 update_velocity_commands(self):
'''
Input:
1. joint angles of the onboard arm
2. Twist from controller: V command - 6x1 array
3. Transformation from e to b
Output:
[5 joint velocities, 4 wheel velocities]'''
Blist = np.array([[0.0, 0.0, 1.0, 0.0, 0.033, 0.0],
[0.0, -1.0, 0.0, -0.5076, 0.0, 0.0],
[0.0, -1.0, 0.0, -0.3526, 0.0, 0.0],
[0.0, -1.0, 0.0, -0.2176, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0, 0.0, 0.0]]).T
thetalist = np.array(self.pose_8_list[3:])
J_arm = mr.JacobianBody(Blist, thetalist)
Adj_Teb = mr.Adjoint(self.Teb)
#test
R_modified = 2.0 * R
F = R_modified / 4.0 * np.array(
[[-1.0 / (L + W), 1.0 / (L + W), 1.0 / (L + W), -1.0 /
(L + W)], [1.0, 1.0, 1.0, 1.0], [-1.0, 1.0, -1.0, 1.0]])
F6_temp = np.vstack((np.zeros((2, 4)), F))
F6 = np.vstack((F6_temp, np.zeros((1, 4))))
J_base = Adj_Teb.dot(F6)
J = np.hstack((J_base, J_arm))
Je_pinv = np.linalg.pinv(J, rcond=1e-3)
u_thetadot = Je_pinv.dot(self.V)
thetadot_u = np.hstack([u_thetadot[4:], u_thetadot[:4]])
#test_joint_limits
if self.if_joint_limit:
new_thetalist = thetalist + thetadot_u[:5] * DELTA_T
for i, theta in enumerate(new_thetalist):
if theta < JOINT_LIMITS[i][0] or theta > JOINT_LIMITS[i][1]:
J_arm[:, i] = np.zeros(6)
J = np.hstack((J_base, J_arm))
Je_pinv = np.linalg.pinv(J, rcond=1e-3)
u_thetadot = Je_pinv.dot(self.V)
thetadot_u = np.hstack([u_thetadot[4:], u_thetadot[:4]])
return thetadot_u
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 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_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 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 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 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 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 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
def update_hand_to_body_transforms(self):
'''
Update frame transforms for transformation matrix and twist
'''
try:
self._tf_listener.waitForTransform('/base',
'/' + self._limb + '_hand',
rospy.Time(),
rospy.Duration(4.0))
(t,
R) = self._tf_listener.lookupTransform('/base',
'/' + self._limb + '_hand',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print 'Warning! Cannot find [{}_hand] tf to [base].'.format(
self._limb)
R = quaternion_matrix(R)
self._T_bh = modern_robotics.RpToTrans(R[:3, :3], t)
self._Ad_bh = modern_robotics.Adjoint(self._T_bh)
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
#! /usr/bin/env python
import rospy
from intera_interface import Limb
import modern_robotics as mr
import numpy as np
import tf
from geometry_msgs.msg import Transform, Twist, TwistStamped
from std_msgs.msg import Float64
import sawyer_MR_description as sw
Blist = sw.Blist
M = sw.M
Slist = mr.Adjoint(M).dot(Blist)
eomg = 0.01 # positive tolerance on end-effector orientation error
ev = 0.001 # positive tolerance on end-effector position error
arm = None
listener = None
home_config = {
'right_j6': -1.3186796875,
'right_j5': 0.5414912109375,
'right_j4': 2.9682451171875,
'right_j3': 1.7662939453125,
'right_j2': -3.0350302734375,
'right_j1': 1.1202939453125,
'right_j0': -0.0001572265625
}
sword_zoffset = 0.25
end_effector_directions = [-1.0, 1.0]
sword_position = None
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
print("Ex3, Tab:")
Tas = mr.TransInv(Tsa)
Tab = np.dot(Tas, Tsb)
print(Tab)
# [[0,-1,0,-1],[-1,0,0,0],[0,0,-1,-2],[0,0,0,1]]
print("Ex5, pb:")
pb = np.append(pb, 1)
Tsb = mr.TransInv(Tbs)
pb = np.dot(Tsb, pb)
pb = np.delete(pb, 3)
print(pb)
# [1,5,-2]
print("Ex7, va:")
Tsa = mr.Adjoint(Tas)
va = np.dot(Tsa, vs)
print(va)
# [1,-3,-2,-3,-1,5]
print("Ex8, theta:")
MatLogTsa = mr.MatrixLog6(Tsa)
vec = mr.so3ToVec(MatLogTsa)
theta = mr.AxisAng6(vec)[-1]
print(theta)
# 2.094395102393196
print("Ex9, Matrix exponential:")
se3 = mr.VecTose3(stheta)
MatExp = mr.MatrixExp6(se3)
print(MatExp)
