J1 = np.append(J_la,J_ra, axis = 0) edot1 = -0.01*e1 Jplus1 = np.linalg.pinv(J1) P1 = eye(robot.nq - 1) - Jplus1*J1 qdot1 = Jplus1 * edot1 Mrh = robot.Mrh(q) e2 = Mrh.translation[0:3,0] - pos_rw_des J2 = Mrh.rotation * robot.Jrh(q)[:3,:] Jplus2 = npl.pinv(J2) e2 = e2 edot2 = -0.01*e2 J2 = J2 Jplus2 = np.linalg.pinv(J2) qdot2 = qdot1 + npl.pinv(J2*P1) * (edot2 - J2*qdot1) qdot = qdot2 robot.increment(q,qdot) robot.display(q) # Debug : x_la = robot.Mrh(q).translation[0,0] y_la = robot.Mrh(q).translation[1,0] x_ra = robot.Mrh(q).translation[0,0] y_ra = robot.Mrh(q).translation[1,0] x_com_des = (x_la + x_ra) /2 y_com_des = (y_la + y_ra) /2 Mcom = robot.com(q) e_x_com = Mcom.item(0) - x_com_des e_y_com = Mcom.item(1) - y_com_des #print "( x/y com : ",Mcom[0,0],Mcom[1,0],")" #print "( x/y com desired : ",x_com_des,y_com_des,")" #print "( Ground contact condition z left/right : ", z_la_des,z_ra_des,")" #print "( contact feet with ground z left/right : ",robot.Mrh(q).translation[2,0],robot.Mrh (q).translation[2,0],")"
# Tutorial 4 from pinocchio.romeo_wrapper import RomeoWrapper robot = RomeoWrapper('/local/bchattop/devel/src/pinocchio/models/romeo.urdf') import pinocchio as se3 from pinocchio.utils import * import numpy as np execfile("Script1.py") q = robot.q0 v = rand(robot.nv) xdes = np.matrix([3.0,1.0,2.0]).T for i in range(1000): Mrh = robot.Mrh(q) e = Mrh.translation[0:3,0] - xdes J = Mrh.rotation * robot.Jrh(q)[:3,:] qdot = -npl.pinv(J)*e robot.increment(q,qdot*1e-2) robot.display(q) updateJointConfiguration(M,'l_wrist')
qp.init(H, g, A, lb, ub, lbA, ubA, np.array([100])) else: qp.hotstart(H, g, A, lb, ub, lbA, ubA, np.array([100])) sol = np.zeros(robot.nv) qp.getPrimalSolution(sol) a = np.matrix(sol).T se3.rnea(robot.model,robot.data,q,v,a) tau_joints = robot.data.tau[6:] ## configuration integration v += np.matrix(a*dt) robot.increment(q,v*dt) ## Display new configuration robot.display(q) display_com_projection(robot,q) time.sleep(0.005) '''### manual resolution ## posture task err_post_pos = Kp_post*(q[7:]-q0[7:]) err_post_vel = Kd_post*(v[6:]-v0[6:]) err_post = eps_post*(err_post_pos + err_post_vel) J_post = np.hstack([np.zeros([robot.nv-6,6]), np.eye(robot.nv-6) ])
class PinocchioControllerAcceleration(object): def __init__(self, dt): self.dt = dt self.robot = RomeoWrapper("/local/tflayols/softwares/pinocchio/models/romeo.urdf") self.robot.initDisplay() self.robot.loadDisplayModel("world/pinocchio", "pinocchio") self.robot.display(self.robot.q0) self.robot.viewer.gui.refresh() self.q = np.copy(self.robot.q0) self.v = np.copy(self.robot.v0) self.a = np.copy(self.robot.v0) self.dq = np.matrix(np.zeros([self.robot.nv, 1])) def controlLfRfCom( self, Lf=[0.0, 0.0, 0.0], dLf=[0.0, 0.0, 0.0], Rf=[0.0, 0.0, 0.0], dRf=[0.0, 0.0, 0.0], Com=[0, 0, 0.63], dCom=[0.0, 0.0, 0.0], ): def robotint(q, dq): M = se3.SE3(se3.Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0]).matrix(), q[:3]) dM = se3.exp(dq[:6]) M = M * dM q[:3] = M.translation q[3:7] = se3.Quaternion(M.rotation).coeffs() q[7:] += dq[6:] def robotdoubleint(q, dq, ddq, dt): dq += dt * ddq robotint(q, dq) def errorInSE3(M, Mdes): """ Compute a 6-dim error vector (6x1 np.maptrix) caracterizing the difference between M and Mdes, both element of SE3. """ error = se3.log(Mdes.inverse() * M) return error.vector() def errorInSE3dyn(M, Mdes, v_frame, v_des): gMl = se3.SE3.Identity() gMl.rotation = M.rotation # Compute error error = errorInSE3(M, Mdes) v_error = v_frame - gMl.actInv(v_des) # ~ a_corriolis = self.robot.acceleration(q,v,0*v,self._link_id, update_geometry) # ~ a_corriolis.linear += np.cross(v_frame.angular.T, v_frame.linear.T).T # ~ a_tot = a_ref - gMl.actInv(a_corriolis) return error, v_error.vector() def errorLinkInSE3dyn(linkId, Mdes, v_des, q, v): # Get the current configuration of the link M = self.robot.position(q, linkId) gMl = se3.SE3.Identity() gMl.rotation = M.rotation v_frame = self.robot.velocity(q, v, linkId) # Compute error error = errorInSE3(M, Mdes) v_error = v_frame - gMl.actInv(v_des) a_corriolis = self.robot.acceleration(q, v, 0 * v, linkId) # ~ a_corriolis.linear += np.cross(v_frame.angular.T, v_frame.linear.T).T # ~ a_tot = gMl.actInv(a_corriolis) #a_ref - gMl.actInv(a_corriolis) a_tot = a_corriolis # ~ dJdq = a_tot.vector() *self.dt dJdq = a_corriolis.vector() return error, v_error.vector(), dJdq def null(A, eps=1e-6): # -12 """Compute a base of the null space of A.""" u, s, vh = np.linalg.svd(A) padding = max(0, np.shape(A)[1] - np.shape(s)[0]) null_mask = np.concatenate(((s <= eps), np.ones((padding,), dtype=bool)), axis=0) null_space = scipy.compress(null_mask, vh, axis=0) return scipy.transpose(null_space) XYZ_LF = np.array(Lf) + np.array([0.0, 0.0, 0.07]) RPY_LF = np.matrix([[0.0], [0.0], [0.0]]) SE3_LF = se3.SE3(se3.utils.rpyToMatrix(RPY_LF), XYZ_LF) XYZ_RF = np.array(Rf) + np.array([0.0, 0.0, 0.07]) RPY_RF = np.matrix([[0.0], [0.0], [0.0]]) SE3_RF = se3.SE3(se3.utils.rpyToMatrix(RPY_RF), XYZ_RF) # _RF________________________________________________________________ Jrf = self.robot.Jrf(self.q).copy() Jrf[:3] = self.robot.Mrf(self.q).rotation * Jrf[:3, :] # Orient in the world base v_ref = se3.se3.Motion(np.matrix([dRf[0], dRf[1], dRf[2], 0.0, 0.0, 0.0]).T) errRf, v_errRf, dJdqRf = errorLinkInSE3dyn(self.robot.rf, SE3_RF, v_ref, self.q, self.v) # _LF________________________________________________________________ Jlf = self.robot.Jlf(self.q).copy() Jlf[:3] = self.robot.Mlf(self.q).rotation * Jlf[:3, :] # Orient in the world base v_ref = se3.se3.Motion(np.matrix([dLf[0], dLf[1], dLf[2], 0.0, 0.0, 0.0]).T) errLf, v_errLf, dJdqLf = errorLinkInSE3dyn(self.robot.lf, SE3_LF, v_ref, self.q, self.v) # _COM_______________________________________________________________ Jcom = self.robot.Jcom(self.q) p_com, v_com, a_com = self.robot.com(self.q, self.v, self.v * 0.0) errCOM = self.robot.com(self.q) - (np.matrix(Com).T) # ~ v_com = Jcom*self.v v_errCOM = v_com - (np.matrix(dCom).T) dJdqCOM = a_com # _Trunk_____________________________________________________________ idx_Trunk = self.robot.index("root") MTrunk0 = self.robot.position(self.robot.q0, idx_Trunk) MTrunk = self.robot.position(self.q, idx_Trunk) # errTrunk=errorInSE3(MTrunk0,MTrunk)[3:6] JTrunk = self.robot.jacobian(self.q, idx_Trunk)[3:6] # v_frame = self.robot.velocity(self.q,self.v,idx_Trunk) # v_ref= se3.se3.Motion(np.matrix([.0,.0,.0,.0,.0,.0]).T) # errTrunk,v_errTrunk = errorInSE3dyn(MTrunk,MTrunk0,v_frame,v_ref) errTrunk, v_errTrunk, dJdqTrunk = errorLinkInSE3dyn(idx_Trunk, MTrunk0, v_ref, self.q, self.v) errTrunk = errTrunk[3:6] v_errTrunk = v_errTrunk[3:6] dJdqTrunk = dJdqTrunk[3:6] # _TASK1 STACK_______________________________________________________ K = 100.0 Kp_foot = K Kp_com = K Kp_Trunk = K Kp_post = K Kd_foot = 2 * np.sqrt(Kp_foot) Kd_com = 2 * np.sqrt(Kp_com) Kd_Trunk = 2 * np.sqrt(Kp_Trunk) Kd_post = 2 * np.sqrt(Kp_post) err1 = np.vstack([Kp_foot * errLf, Kp_foot * errRf, Kp_com * errCOM, Kp_Trunk * errTrunk]) v_err1 = np.vstack([Kd_foot * v_errLf, Kd_foot * v_errRf, Kd_com * v_errCOM, Kd_Trunk * v_errTrunk]) dJdq1 = np.vstack([dJdqLf, dJdqRf, dJdqCOM, dJdqTrunk]) J1 = np.vstack([Jlf, Jrf, Jcom, JTrunk]) # _TASK2 STACK_______________________________________________________ # _Posture___________________________________________________________ Jpost = np.hstack([zero([self.robot.nv - 6, 6]), eye(self.robot.nv - 6)]) errPost = Kp_post * (self.q - self.robot.q0)[7:] v_errPost = Kd_post * (self.v - self.robot.v0)[6:] errpost = -1 * (self.q - self.robot.q0)[7:] err2 = errPost v_err2 = v_errPost J2 = Jpost # Hierarchical solve_________________________________________________ qddot = npl.pinv(J1) * (-1.0 * err1 - 1.0 * v_err1 - 1.0 * dJdq1) Z = null(J1) qddot += Z * npl.pinv(J2 * Z) * (-(1.0 * err2 + 1.0 * v_err2) - J2 * qddot) self.a = qddot self.v += np.matrix(self.a * self.dt) self.robot.increment(self.q, np.matrix(self.v * self.dt)) # ~ robotdoubleint(self.q,self.dq,qddot,self.dt) # ~ self.v=self.dq/self.dt self.robot.display(self.q) self.robot.viewer.gui.refresh() return self.robot.com(self.q), Jcom * self.v, errCOM, v_errCOM