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
