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)