def extrinsic(self,X_,x=0.,y=0.,thz=0.): """ X = extrinsic(X_) """ z,thx,thy,dx_,dy_,dz,dthx,dthy,dthz = X_ rot = geom.euler([0.,0.,thz]) dx,dy,_ = np.dot([dx_,dy_,0.],rot.T) X = np.r_[x,y,z,thx,thy,thz,dx,dy,dz,dthx,dthy,dthz] return X
def Rot(self,t,x,p): """ rot = Rot(...) rot - 3 x 3 - body rotation matrix in lab frame """ th = x[3:6] # x,y,z rotation angles rot = geom.euler(th) return rot
def intrinsic(self,X): """ X_ = intrinsic(...) """ q,dq = X[:6],X[6:] x,y,z_,thx_,thy_,thz = q dx,dy,dz_,dthx_,dthy_,dthz_ = dq rot = geom.euler([0.,0.,thz]) dx_,dy_,_ = np.dot([dx,dy,0.],rot) X_ = np.r_[z_,thx_,thy_,dx_,dy_,dz_,dthx_,dthy_,dthz_] return X_
def saggital(self,X): """ X_ = saggital(...) """ q,dq = X[:6],X[6:] x_,_,z_,_,thy_,thz = q dx,dy,dz_,_,dthy_,_ = dq rot = geom.euler([0.,0.,thz]) dx_,_,_ = np.dot([dx,dy,0.],rot) X_ = np.r_[x_,z_,thy_,dx_,dz_,dthy_] return X_
def Foot(self,t,x,p,pos=None,rot=None,debug=False): """ foot = Foot(...) foot - n x 3 - foot locations in world frame for each leg """ # body pos,rot in world frame if pos is None: pos = x[0:3] if rot is None: rot = self.Rot(t,x,p) # stance legs foot = p.foot.copy() # swing legs for l in (1.-p.b).nonzero()[0]: R = geom.euler([0.,-p.psi[l],-p.zeta[l]]).T f = p.hip[l] - np.dot( p.lamb[l]*p.ez, R.T ) foot[l] = pos + np.dot( f, rot.T ) if debug: print R, f, pos, rot #1/0 return foot