def addRobot(self, robot): if(ENABLE_VIEWER): self.robots[robot.name] = robot nodeName = "world/"+robot.name print "adding robot: "+robot.name self.loadDisplayModel(nodeName, "pinocchio", self.robots[robot.name]); se3.framesKinematics(self.robots[robot.name].model, self.robots[robot.name].data, self.robots[robot.name].q0) self.updateRobotConfig(self.robots[robot.name].q0,robot.name)
def display(self, q): RobotWrapper.display(self, q) M1 = self.data.oMi[1] self.viewer.gui.applyConfiguration('world/mobilebasis', pin.se3ToXYZQUATtuple(M1)) self.viewer.gui.applyConfiguration('world/mobilewheel1', pin.se3ToXYZQUATtuple(M1)) self.viewer.gui.applyConfiguration('world/mobilewheel2', pin.se3ToXYZQUATtuple(M1)) self.viewer.gui.refresh() pin.framesKinematics(self.model, self.data) self.viewer.gui.applyConfiguration('world/framebasis', pin.se3ToXYZQUATtuple(self.data.oMf[-2])) self.viewer.gui.applyConfiguration('world/frametool', pin.se3ToXYZQUATtuple(self.data.oMf[-1])) self.viewer.gui.refresh()
def display(self, q): RobotWrapper.display(self, q) M1 = self.data.oMi[1] self.viewer.gui.applyConfiguration('world/mobilebasis', M2gv(M1)) self.viewer.gui.applyConfiguration('world/mobilewheel1', M2gv(M1)) self.viewer.gui.applyConfiguration('world/mobilewheel2', M2gv(M1)) self.viewer.gui.refresh() se3.framesKinematics(self.model, self.data) self.viewer.gui.applyConfiguration('world/framebasis', M2gv(self.data.oMf[-2])) self.viewer.gui.applyConfiguration('world/frametool', M2gv(self.data.oMf[-1])) self.viewer.gui.refresh()
def update(self, q): se3.computeAllTerms(self.model, self.data, self.q, self.v) #se3.forwardKinematics(self.model, self.data, q, self.v, self.a) #- se3::forwardKinematics #- se3::crba #- se3::nonLinearEffects #- se3::computeJacobians #- se3::centerOfMass #- se3::jacobianCenterOfMass #- se3::kineticEnergy #- se3::potentialEnergy se3.framesKinematics(self.model, self.data, q) se3.computeJacobians(self.model, self.data, q) se3.rnea(self.model, self.data, q, self.v, self.a) self.biais(self.q, self.v) self.q = q.copy()
def computePosition(self, q): pin.framesKinematics(self.robot.model, self.robot.data, q) frame_id = self.robot.model.getFrameId('lf_foot') self.pos['lf_foot'] = self.getFramePosition(frame_id).translation frame_id = self.robot.model.getFrameId('lh_foot') self.pos['lh_foot'] = self.getFramePosition(frame_id).translation frame_id = self.robot.model.getFrameId('rf_foot') self.pos['rf_foot'] = self.getFramePosition(frame_id).translation frame_id = self.robot.model.getFrameId('rh_foot') self.pos['rh_foot'] = self.getFramePosition(frame_id).translation return self.pos
def step(self, q, vq, tauq, dt=None): if dt is None: dt = self.dt robot = self.robot NQ, NV, NB, RF, LF, RK, LK = self.NQ, self.NV, self.NB, self.RF, self.LF, self.RK, self.LK # --- Simu se3.forwardKinematics(robot.model, robot.data, q, v) se3.framesKinematics(robot.model, robot.data) Mrf = self.Mrf0.inverse() * robot.data.oMf[RF] vrf = robot.model.frames[RF].placement.inverse() * robot.data.v[RK] Mlf = self.Mlf0.inverse() * robot.data.oMf[LF] vlf = robot.model.frames[LF].placement.inverse() * robot.data.v[LK] qrf = np.vstack( [Mrf.translation[1:], se3.rpy.matrixToRpy(Mrf.rotation)[0]]) qlf = np.vstack( [Mlf.translation[1:], se3.rpy.matrixToRpy(Mlf.rotation)[0]]) frf = self.frf # Buffer where to store right force frf[[1, 2, 3]] = self.Krf * qrf # Right force in effector frame rf0_frf = se3.Force(frf) # Force in rf0 frame rk_frf = (robot.data.oMi[RK].inverse() * self.Mrf0).act( rf0_frf) # Spring force in R-knee frame. flf = self.flf # Buffer where to store left force flf[[1, 2, 3]] = self.Klf * qlf # Left force in effector frame lf0_flf = se3.Force(flf) # Force in lf0 frame lk_flf = (robot.data.oMi[LK].inverse() * self.Mlf0).act( lf0_flf) # Spring force in L-knee frame. self.forces = ForceDict({ RK: rk_frf, LK: lk_flf }, NB) # Argument to ABA aq = se3.aba(robot.model, robot.data, q, vq, tauq, self.forces) vq += aq * dt q = se3.integrate(robot.model, q, vq * dt) self.aq = aq return q, vq
def __init__(self, robot, q0=None, dt=1e-3, ndt=10): ''' Initialize from a Hrp2Reduced robot model, an initial configuration, a timestep dt and a number of Eurler integration step ndt. The <simu> method (later defined) processes <ndt> steps, each of them lasting <dt>/<ndt> seconds, (i.e. total integration time when calling simu is <dt> seconds). <q0> should be an attribute of robot if it is not given. ''' self.frf = zero(6) self.flf = zero(6) self.dt = dt # Time step self.ndt = ndt # Discretization (number of calls of step per time step) self.robot = robot self.NQ = robot.model.nq self.NV = robot.model.nv self.NB = robot.model.nbodies self.RF = robot.model.getFrameId('rankle') self.LF = robot.model.getFrameId('lankle') self.RK = robot.model.frames[self.RF].parent self.LK = robot.model.frames[self.LF].parent NQ, NV, NB, RF, LF, RK, LK = self.NQ, self.NV, self.NB, self.RF, self.LF, self.RK, self.LK if q0 is None: q0 = robot.q0 se3.forwardKinematics(robot.model, robot.data, q0) se3.framesKinematics(robot.model, robot.data) self.Mrf0 = robot.data.oMf[RF].copy( ) # Initial (i.e. 0-load) position of the R spring. self.Mlf0 = robot.data.oMf[LF].copy( ) # Initial (i.e. 0-load) position of the L spring. self.Krf = -np.diagflat([20000., 200000., 00. ]) # Stiffness of the Right spring self.Klf = -np.diagflat([20000., 200000., 00. ]) # Stiffness of the Left spring robot.viewer.addXYZaxis('world/mrf', [1., .6, .6, 1.], .03, .1) robot.viewer.addXYZaxis('world/mlf', [.6, .6, 1., 1.], .03, .1) robot.viewer.applyConfiguration('world/mrf', se3ToXYZQUAT(self.Mrf0)) robot.viewer.applyConfiguration('world/mlf', se3ToXYZQUAT(self.Mlf0))
def inverse_kinematics(self, fid, xdes, q0): """ Method not in use right now, but is here with the intention of using pinocchio for inverse kinematics instead of using the in-house IK solver of pybullet. """ raise NotImplementedError() dt = 1.0e-3 pinocchio.computeJointJacobians( self.robot_model, self.data, q0, ) pinocchio.framesKinematics( self.robot_model, self.data, q0, ) pinocchio.framesForwardKinematics( self.robot_model, self.data, q0, ) Ji = pinocchio.getFrameJacobian( self.robot_model, self.data, fid, pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED, )[:3, :] xcurrent = self.data.oMf[fid].translation try: Jinv = np.linalg.inv(Ji) except Exception: Jinv = np.linalg.pinv(Ji) dq = Jinv.dot(xdes - xcurrent) qnext = pinocchio.integrate(self.robot_model, q0, dt * dq) return qnext
def tip(self, q): '''Return the altitude of pendulum tip''' se3.framesKinematics(self.model, self.data, q) return self.data.oMf[1].translation[2, 0]
def framesKinematics(self, q): se3.framesKinematics(self.model, self.data, q)
def compute_ddf_approximations(robot): ''' Compute ddf based on the following dynamics equation: ddf_1 = K*(-Upsilon*f - dJ*v + J*M_inv*h - J*Minv*S^T*tau) ''' ddf_list = {} for i in range(1,11): ddf_list[str(i)] = matlib.empty((nf,T))*np.nan dyn_res = matlib.empty((nv,T))*np.nan # residual of robot dynamics S = matlib.zeros((na,nv)) S[:,nv-na:] = matlib.eye(na) dq_cmd_integral = q_cmd[:,0].copy() eig_J_Minv_ST_Mj_diag_JSTpinv = np.empty((nf,T)) for t in range(T): se3.computeAllTerms(robot.model, robot.data, q[:,t], v[:,t]) se3.framesKinematics(robot.model, robot.data, q[:,t]) M = robot.data.M #(7,7) h = robot.data.nle Jl,Jr = robot.get_Jl_Jr_world(q[:,t], False) J = np.vstack([Jl[1:3],Jr[1:3]]) # (4, 7) driftLF,driftRF = robot.get_driftLF_driftRF_world(q[:,t], v[:,t], False) dJ_v = np.vstack([driftLF.vector[1:3], driftRF.vector[1:3]]) Mj = robot.data.M[3:,3:] #(7,7) Minv = np.linalg.inv(M) Upsilon = J*Minv*J.T ddf_list['1'][:,t] = K*(-Upsilon*f[:,t] - dJ_v + J*Minv*h - J*Minv*S.T*tau[:,t]) tau_2 = Kp_pos*(q_cmd[:,t]-q[4:,t]) - Kd_pos*v[3:,t] ddf_list['2'][:,t] = K*(-Upsilon*f[:,t] - dJ_v + J*Minv*h - J*Minv*S.T*tau_2) # simplified version of controller (without postural task) feet_v_ref = - Kf*(f_des[:,t] - f[:,t]) JSTpinv = np.linalg.pinv(J*S.T) dq_cmd = JSTpinv * feet_v_ref dq_cmd_integral += dt*dq_cmd if(t==0): dq_cmd_integral = q_cmd[:,0] tau_3 = Kp_pos*(dq_cmd_integral-q[4:,t]) - Kd_pos*v[3:,t] ddf_list['3'][:,t] = K*(-Upsilon*f[:,t] - dJ_v + J*Minv*h - J*Minv*S.T*tau_3) # compute eigenvalues of J*Minv*S.T*Mj_diag*JSTpinv to check its positive-definiteness A = J*Minv*S.T*Mj_diag*JSTpinv eig_J_Minv_ST_Mj_diag_JSTpinv[:,t] = eigvals(A) B = Minv*S.T*Mj_diag*S B_bar = B - matlib.eye(B.shape[0]) # move the pseudoinverse out of the integral if(t==0): feet_v_ref_integral_0 = J*S.T*q_cmd[:,0] e_f_integral_0 = Kf_inv*feet_v_ref_integral_0 feet_v_ref_integral = feet_v_ref_integral_0 else: feet_v_ref_integral += dt*feet_v_ref tau_4 = Kp_pos*(JSTpinv*feet_v_ref_integral - q[4:,t]) - Kd_pos*v[3:,t] ddf_list['4'][:,t] = K*(-Upsilon*f[:,t] - dJ_v + J*Minv*h - J*Minv*S.T*tau_4) # equivalent reformulation tau_5 = kp_bar*A*feet_v_ref_integral - kp_bar*J*B*q[1:,t] + kd_bar*K_inv*df[:,t] - kd_bar*J*B_bar*v[:,t] ddf_list['5'][:,t] = K*(-Upsilon*f[:,t] - dJ_v + J*Minv*h - tau_5) # remove dJ_v e_f_integral = Kf_inv*feet_v_ref_integral tau_6 = kp_bar*A*Kf*e_f_integral - kp_bar*J*B*q[1:,t] + kd_bar*K_inv*df[:,t] - kd_bar*J*B_bar*v[:,t] ddf_list['6'][:,t] = K*(-Upsilon*f[:,t] + J*Minv*h - tau_6) # equivalent reformulation ddf_list['7'][:,t] = -kd_bar*df[:,t] -K*Upsilon*f[:,t] -kp_bar*K*A*Kf*e_f_integral +K*J*Minv*h +kp_bar*K*J*B*q[1:,t] +kd_bar*K*J*B_bar*v[:,t] # neglect B_bar ddf_list['8'][:,t] = -kd_bar*df[:,t] -K*Upsilon*f[:,t] -kp_bar*K*A*Kf*e_f_integral +K*J*Minv*h +kp_bar*K*J*B*q[1:,t] # neglect J*B*q ddf_list['9'][:,t] = -kd_bar*df[:,t] -K*Upsilon*f[:,t] -kp_bar*K*A*Kf*e_f_integral +K*J*Minv*h +kd_bar*K*J*B_bar*v[:,t] # neglect K*J*Minv*h and assume zero integral at start ddf_list['10'][:,t] = -kd_bar*df[:,t] -K*Upsilon*f[:,t] -kp_bar*K*A*Kf*(e_f_integral-e_f_integral_0) +kp_bar*K*J*B*q[1:,t] +kd_bar*K*J*B_bar*v[:,t] dyn_res[:,t] = M*dv[:,t] + h - J.T*f[:,t] - S.T*tau[:,t] if(t*dt>=T_DISTURB_BEGIN and t*dt<=T_DISTURB_END): ddf[:,t] = np.nan dyn_res[:,t] = np.nan for key in ddf_list: ddf_list[key][:,t] = np.nan # compute errors for key in np.sort(ddf_list.keys()): print "Max error ddf[%s]: %.0f"%(key, np.nanmax(np.abs(ddf-ddf_list[key]))) ncols = 2 nsubplots = nf nrows = int(nsubplots/ncols) for key in np.sort(ddf_list.keys()): ff, ax = plt.subplots(nrows, ncols, sharex=True); ax = ax.reshape(nsubplots) for i in range(nf): ax[i].plot(time, ddf[i,:].A1, '-', label='Real') ax[i].plot(time, ddf_list[key][i,:].A1, '--', label='Approx '+key) ax[i].legend() ax[i].set_title('ddf '+str(i)) # ax[i].set_ylabel('N/s*s') ax[-1].set_xlabel('Time [s]') if(PLOT_EIGENVALUES_J_Minv_ST_Mj_diag_JSTpinv): plt.figure() plt.plot(eig_J_Minv_ST_Mj_diag_JSTpinv.T) plt.title('Eigenvalues of J*Minv*S.T*Mj_diag*(J*S.T)^+') if(PLOT_DYNAMICS_RESIDUAL): ff, ax = plt.subplots(1, 1, sharex=True); for i in range(nv): ax.plot(time, dyn_res[i,:].A1, '-', label='Axis '+str(i)) ax.set_title('Dynamics residual') ax.set_ylabel('Nm') ax.set_xlabel('Time [s]') ax.legend() return ddf_list
def tip(self,q): '''Return the altitude of pendulum tip''' se3.framesKinematics(self.model,self.data,q) return self.data.oMf[1].translation[2,0]
pkg = '/home/student/models/' urdf = pkg + 'ur_description/urdf/ur5_gripper.urdf' robot = MobileRobotWrapper(urdf, [ pkg, ]) robot.initDisplay(loadModel=True) #robot.viewer.gui.addFloor('world/floor') NQ = robot.model.nq NV = robot.model.nv q = se3.randomConfiguration(robot.model, zero(NQ) - np.pi, zero(NQ) + np.pi) vq = rand(NV) robot.display(q) from time import sleep for i in range(10000): robot.increment(q, vq / 100) robot.display(q) sleep(.01) IDX_TOOL = 24 IDX_BASIS = 23 se3.framesKinematics(robot.model, robot.data) Mtool = robot.data.oMf[IDX_TOOL] Mbasis = robot.data.oMf[IDX_BASIS]
def updateRobotConfig(self, q): se3.computeAllTerms(self.robot.model, self.robot.data, q, self.robot.v) se3.framesKinematics(self.robot.model, self.robot.data, q) se3.computeJacobians(self.robot.model, self.robot.data, q)