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 feedback_control(self, x_d, x_d_next, x): """Returns a commanded end effector twist in the end effector frame given the current configuration, reference configurations and gains Input x_d (array in SE(3)): The current end effector reference configuration x_d_next (array in SE(3): the end effector reference configuration at the next timestep in the reference trajectory x (array in SE(3)): the actual end effector configuration Output v: the end effector twist in the end-effector frame """ x_err = se3ToVec(MatrixLog6(np.dot(TransInv(x), x_d))) # calculate feedforward reference twist v_d = se3ToVec( (1 / self.dt) * MatrixLog6(np.dot(TransInv(x_d), x_d_next))) print(f' x_err {x_err}') print(v_d) v = np.dot(Adjoint(np.dot(TransInv(x), x_d)), v_d) + np.dot( self.k_p, x_err) + np.dot(self.k_i, self.x_err_integral) self.x_err_integral = np.add(self.x_err_integral, x_err) return v, x_err
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 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 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 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 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 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 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, 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 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 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 IKinBodyIterates(Blist, M, T, thetalist0, eomg, ev): H1 = .1519 H2 = .08535 W1 = .11235 W2 = .0819 L1 = .24365 L2 = .21325 B1 = np.array([0,1,0,W1+W2,0,L1+L2]) B2 = np.array([0,0,1,H2,-L1-L2, 0]) B3 = np.array([0,0,1,H2,-L2,0]) B4 = np.array([0,0,1,H2,0,0]) B5 = np.array([0,-1,0,-W2,0,0]) B6 = np.array([0,0,1,0,0,0]) Blist = np.array([B1,B2,B3,B4,B5,B6]) Blist = Blist.T # thetalist0 = np.array([1.707,-1.578,0,-1.514,-0.032,1.514]) thetalist0 = np.array([-0.641,-0.419,-0.676,0,0.87,0]) M = np.array([[-1, 0, 0, 0.45671838521957], [0, 0, 1, 0.19418559968472], [0, 1, 0, 0.07377764582634], [0,0,0,1]]) T = np.array([[0.88665968179703, -0.43939724564552, -0.14409957826138, 0.13819913566113], [0.41775861382484, 0.6275218129158, 0.6570343375206, 0.036001659929752], [-0.19827343523502, -0.64276474714279, 0.73996025323868, 0.60956245660782], [0,0,0,1]]) # M = np.array([[-1, 0, 0, L1+L2], # [0, 0, 1, W1+W2+0.2816], # [0, 1, 0, H1-H2], # [0,0,0,1]]) # T = np.array([[ 0, 0,-1,-0.34298], # [ 0,-1, 0,-0.098685], # [-1, 0, 0, 1.1109], # [ 0, 0, 0, 1]]) # T = np.array([[0.003,0.104,-0.995,-0.391], # [0.008,-0.995,-0.104,-0.044], # [-1,-0.007,-0.004,0.699], # [0,0,0,1]]) eomg = 0.01 ev = 0.01 ## done with initial values thetalist = np.array(thetalist0).copy() iter_thetas = np.array(thetalist.copy()) i = 0 Vb = mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(mr.FKinBody(M, Blist, thetalist)), T))) eomg_act = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) ev_act = np.linalg.norm([Vb[3], Vb[4], Vb[5]]) err = eomg_act > eomg or ev_act > ev T_act = mr.FKinBody(M, Blist, thetalist) print(" Iteration %d:\n\n \ Joint Vector:\n \ %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f\n\n \ SE(3) End-Effector Configuration:\n \ %6.4f %6.4f %6.4f %6.4f\n \ %6.4f %6.4f %6.4f %6.4f\n \ %6.4f %6.4f %6.4f %6.4f\n \ %6.4f %6.4f %6.4f %6.4f\n\n \ Error Twist Vb:\n \ %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f\n\n \ Angular Error Magnitude || omega_b ||: %6.5f\n \ Linear Error Magnitude || v_b ||: %6.5f\n" % (i,thetalist[0],thetalist[1],thetalist[2],thetalist[3],thetalist[4],thetalist[5], T_act[0,0],T_act[0,1],T_act[0,2],T_act[0,3],T_act[1,0],T_act[1,1],T_act[1,2],T_act[1,3],T_act[2,0],T_act[2,1],T_act[2,2],T_act[2,3], T_act[3,0],T_act[3,1],T_act[3,2],T_act[3,3],Vb[0],Vb[1],Vb[2],Vb[3],Vb[4],Vb[5],eomg_act,ev_act)) while err: i = i + 1 thetalist = thetalist + np.dot(np.linalg.pinv(mr.JacobianBody(Blist, thetalist)), Vb) iter_thetas = np.vstack([iter_thetas,thetalist]) Vb = mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(mr.FKinBody(M, Blist, thetalist)), T))) T_act = mr.FKinBody(M, Blist, thetalist) eomg_act = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) ev_act = np.linalg.norm([Vb[3], Vb[4], Vb[5]]) err = eomg_act > eomg or ev_act > ev print(" Iteration %d:\n\n \ Joint Vector:\n \ %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f\n\n \ SE(3) End-Effector Configuration:\n \ %6.4f %6.4f %6.4f %6.4f\n \ %6.4f %6.4f %6.4f %6.4f\n \ %6.4f %6.4f %6.4f %6.4f\n \ %6.4f %6.4f %6.4f %6.4f\n\n \ Error Twist Vb:\n \ %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f\n\n \ Angular Error Magnitude || omega_b ||: %6.5f\n \ Linear Error Magnitude || v_b ||: %6.5f\n" % (i,thetalist[0],thetalist[1],thetalist[2],thetalist[3],thetalist[4],thetalist[5], T_act[0,0],T_act[0,1],T_act[0,2],T_act[0,3],T_act[1,0],T_act[1,1],T_act[1,2],T_act[1,3],T_act[2,0],T_act[2,1],T_act[2,2],T_act[2,3], T_act[3,0],T_act[3,1],T_act[3,2],T_act[3,3],Vb[0],Vb[1],Vb[2],Vb[3],Vb[4],Vb[5],eomg_act,ev_act)) f = open("output.csv", "w") for i in range(len(iter_thetas)): output = "%7.6f,%7.6f,%7.6f,%7.6f,%7.6f,%7.6f\n" % (iter_thetas[i,0], iter_thetas[i,1], iter_thetas[i,2], iter_thetas[i,3], iter_thetas[i,4], iter_thetas[i,5]) f.write(output) f.close() return iter_thetas, not err
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