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; }