Commit b11567be by Rohan Budhiraja

### run framesforwardkinematics before finding position signal value

parent 39a3db9c
 from dynamic import Dynamic from angle_estimator import AngleEstimator from zmp_from_forces import ZmpFromForces import numpy as np from numpy import arctan2, arcsin, sin, cos, sqrt DynamicOld = Dynamic ... ... @@ -13,3 +15,54 @@ class Dynamic (DynamicOld): def setModel(self, pinocchio_model): dynamic.wrap.set_pinocchio_model(self.obj,pinocchio_model) return def fromSotToPinocchio(q_sot, freeflyer=True): if freeflyer: [r,p,y] = q_sot[3:6] cr = cos(r) cp = cos(p) cy = cos(y) sr = sin(r) sp = sin(p) sy = sin(y) rotmat = np.matrix([[cy*cp, cy*sp*sr-sy*cr, cy*sp*cr+sy*sr], [sy*cp, sy*sp*sr+cy*cr, sy*sp*cr-cy*sr], [-sp, cp*sr, cp*cr]]) d0 = rotmat[0,0] d1 = rotmat[1,1] d2 = rotmat[2,2] rr = 1.0+d0+d1+d2 if rr>0: s = 0.5 / sqrt(rr) _x = (rotmat[2,1] - rotmat[1,2]) * s _y = (rotmat[0,2] - rotmat[2,0]) * s _z = (rotmat[1,0] - rotmat[0,1]) * s _r = 0.25 / s else: #Trace is less than zero, so need to determine which #major diagonal is largest if ((d0 > d1) and (d0 > d2)): s = 0.5 / sqrt(1 + d0 - d1 - d2) _x = 0.5 * s _y = (rotmat[0,1] + rotmat[1,0]) * s _z = (rotmat[0,2] + rotmat[2,0]) * s _r = (rotmat[1,2] + rotmat[2,1]) * s elif (d1 > d2): s = 0.5 / sqrt(1 + d0 - d1 - d2) _x = (rotmat[0,1] + rotmat[1,0]) * s _y = 0.5 * s _z = (rotmat[1,2] + rotmat[2,1]) * s _r = (rotmat[0,2] + rotmat[2,0]) * s else: s = 0.5 / sqrt(1 + d0 - d1 - d2) _x = (rotmat[0,2] + rotmat[2,0]) * s _y = (rotmat[1,2] + rotmat[2,1]) * s _z = 0.5 * s _r = (rotmat[0,1] + rotmat[1,0]) * s return np.matrix([q_sot[0:3]+(_x,_y,_z,_r)+q_sot[6:]]) else: return np.matrix([q_sot[0:]])
 ... ... @@ -29,6 +29,7 @@ #include #include #include #include #include ... ... @@ -287,7 +288,6 @@ dg::Vector Dynamic::getPinocchioPos(int time) if( freeFlyerPositionSIN) { dg::Vector qFF=freeFlyerPositionSIN.access(time); q.resize(qJoints.size() + 7); Eigen::Quaternion quat = Eigen::AngleAxisd(qFF(5),Eigen::Vector3d::UnitZ())* ... ... @@ -308,15 +308,14 @@ dg::Vector Dynamic::getPinocchioPos(int time) q << qFF(0),qFF(1),qFF(2), quat.x(),quat.y(),quat.z(),quat.w(), qJoints.segment(6,qJoints.size()-6); assert(q.size() == m_model->nq); } else { q.resize(qJoints.size()); q=qJoints; } sotDEBUGOUT(15) <<"Position out"<getPinocchioPos(time)); se3::Data::Matrix6x m_output = Eigen::MatrixXd::Zero(6,m_model->nv); std::string temp; if(isFrame){ se3::framesForwardKinematics(*m_model,*m_data); se3::getFrameJacobian(*m_model,*m_data,(se3::Model::Index)jointId,m_output); temp = m_model->getFrameName((se3::Model::Index)jointId); } else { temp = m_model->getJointName((se3::Model::Index)jointId); se3::getJacobian(*m_model,*m_data,(se3::Model::Index)jointId,m_output); } else se3::getJacobian(*m_model,*m_data,(se3::Model::Index)jointId,m_output); res = m_output; sotDEBUG(25) << "EndEffJacobian for "<oMf[jointId].toHomogeneousMatrix(); temp = m_model->getFrameName((se3::Model::Index)jointId); } else{ res.matrix()= m_data->oMi[jointId].toHomogeneousMatrix(); temp = m_model->getJointName((se3::Model::Index)jointId); } else res.matrix()= m_data->oMi[jointId].toHomogeneousMatrix(); sotDEBUG(25)<<"For "<