void Model::
update(State const & state)
{
setState(state);
updateKinematics();
updateDynamics();
}
void SingleSupportModel::doInverseDynamics(bool normalize, DataSource source)
{
if(m_coeff == 0 && normalize)
return;
double coeff = m_normalizedCoeff;
if(!normalize)
coeff = 1.0;
updateKinematics(source);
tf::Vector3 grav(0, 0, -9.81);
grav = tf::Transform(m_model->robotOrientation()).inverse() * grav;
// Transform grav to base coordinates
if(m_trunkJoint != -1)
{
const Math::SpatialTransform X = X_base[m_trunkJoint];
gravity = X.E.transpose() * Math::Vector3d(grav.x(), grav.y(), grav.z());
}
RigidBodyDynamics::InverseDynamics(*this, m_q, m_qdot, m_qdotdot, m_tau, 0);
for(size_t i = 0; i < m_joints.size(); ++i)
{
if(!m_joints[i])
continue;
m_joints[i]->feedback.modelTorque += coeff * m_tau[i];
}
}
void SingleSupportModel::computeCOM()
{
Math::Vector3d com = Math::Vector3d::Zero();
double mass = 0;
updateKinematics(MeasurementData);
for(size_t i = 0; i < mBodies.size(); ++i)
{
const Math::SpatialTransform& X = X_base[i];
const RigidBodyDynamics::Body& body = mBodies[i];
Math::Vector3d bodyCom = X.E.transpose() * body.mCenterOfMass + X.r;
com += body.mMass * bodyCom;
mass += body.mMass;
}
m_com = com / mass;
}
void Physics::update(int delta){
float dt=delta/1000.0f;
// float mindt = 0.005f;
// float leftover=(dt>mindt)?dt-mindt:0;
//calculate the amount of force being applied to object for a split second
Vector appliedForce(impulseForce_+bodyForce_);
acceleration_=appliedForce/mass_;
//reduce the impulse force to 0
// impulse moments
Matrix R = rotation();
Matrix RT = R.transpose();
Vector ml = moment_ * RT;
Vector wl = angularVelocity() * RT;
setAngularAcceleration((ml - cross(wl*momentInertia_ ,wl)) * invMomentInertia_ * R);
updateKinematics(dt);
moment_ = Vector();
impulseForce_=Vector();
//setAngularAcceleration(Vector());
//calculate a new acceleration for the rest of the frame based soley on the body force
//acceleration_=bodyForce_/mass_;
}
bool GSystem::_getReady()
{
std::list<GJoint *>::iterator iter_pjoint;
std::list<GBody *>::iterator iter_pbody;
// check joints and bodies (This should be placed after _findParentChildRelation().)
for (iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); iter_pjoint++) {
if ( !(*iter_pjoint)->getReady() ) return false;
}
for (iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); iter_pbody++) {
if ( !(*iter_pbody)->getReady() ) return false;
}
// update kinematics
updateKinematics();
// load joint information on each body (one time call)
for (iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); iter_pbody++) {
(*iter_pbody)->load_base_joint_info();
}
return true;
}
/// may not need this it is in the base class
/// blocking call, call in separate thread, just allocates memory
void run_one(){
updateKinematics();
}
/**
* @function main
*/
int main( int argc, char* argv[] ) {
// Set kinematics
setKinematics();
// Open Communication
openComm();
// Marker detector
mMd.setMarkerSize(5);
// Open device
cv::VideoCapture capture( cv::CAP_OPENNI2 );
if( !capture.isOpened() ) {
std::cout << "/t * Could not open the capture object"<<std::endl;
return -1;
}
printf("\t * Opened device \n");
capture.set( cv::CAP_PROP_OPENNI2_MIRROR, 0.0 ); // off
capture.set( cv::CAP_PROP_OPENNI_REGISTRATION, -1.0 ); // on
printf("\t * RGB dimensions: (%f,%f), Depth dimensions: (%f,%f) \n",
capture.get( cv::CAP_OPENNI_IMAGE_GENERATOR + cv::CAP_PROP_FRAME_WIDTH ),
capture.get( cv::CAP_OPENNI_IMAGE_GENERATOR + cv::CAP_PROP_FRAME_HEIGHT ),
capture.get( cv::CAP_OPENNI_DEPTH_GENERATOR + cv::CAP_PROP_FRAME_WIDTH ),
capture.get( cv::CAP_OPENNI_DEPTH_GENERATOR + cv::CAP_PROP_FRAME_HEIGHT ) );
// Set control panel
int value;
cv::namedWindow( mWindowName );
// Set mouse callback
cv::setMouseCallback( mWindowName, onMouse, 0 );
// Set buttons
cv::createTrackbar( "**********", NULL, &value, 255, NULL );
cv::createButton( "Grab snapshot", grabSnapshot,
NULL, cv::QT_PUSH_BUTTON,
false );
cv::createButton( "Show snapshot", showSnapshot,
NULL, cv::QT_PUSH_BUTTON,
false );
cv::createTrackbar( "----------", NULL, &value, 255, NULL );
cv::createButton( "Store snapshot", storeSnapshot,
NULL, cv::QT_PUSH_BUTTON,
false );
cv::createButton( "Discard snapshot", discardSnapshot,
NULL, cv::QT_PUSH_BUTTON,
false );
cv::createTrackbar( "xxxxxxxxx", NULL, &value, 255, NULL );
cv::createButton( "Calibrate", calibrate,
NULL, cv::QT_PUSH_BUTTON,
false );
// Loop
for(;;) {
if( !capture.grab() ) {
std::cout << "\t * [DAMN] Could not grab a frame" << std::endl;
return -1;
}
updateKinematics();
updateTf();
capture.retrieve( mRgbImg, cv::CAP_OPENNI_BGR_IMAGE );
capture.retrieve( mDepthImg, cv::CAP_OPENNI_POINT_CLOUD_MAP );
mMd.detect( mRgbImg );
cv::imshow( mWindowName, mRgbImg );
char k = cv::waitKey(30);
if( k == 'q' ) {
printf("\t * [PRESSED ESC] Finishing the program \n");
break;
}
aa_mem_region_local_release();
} // end for
return 0;
}
