BulletOpenGLViewer::BulletOpenGLViewer(DynamicsWorldPtr world) { SIMDYNAMICS_ASSERT(world); m_sundirection = btVector3(1, 1, -2) * 1000; bulletEngine = boost::dynamic_pointer_cast<BulletEngine>(world->getEngine()); SIMDYNAMICS_ASSERT(bulletEngine); setTexturing(true); setShadows(true); // set Bullet world (defined in bullet's OpenGL helpers) m_dynamicsWorld = bulletEngine->getBulletWorld(); m_dynamicsWorld->setDebugDrawer(&debugDrawer); // set up vector for camera setCameraDistance(btScalar(3.)); setCameraForwardAxis(1); btVector3 up(0, 0, btScalar(1.)); setCameraUp(up); clientResetScene(); }
void SimoxMotionState::setGlobalPoseSimox( const Eigen::Matrix4f& worldPose ) { if (!sceneObject) return; // inv com as matrix4f Eigen::Matrix4f comLocal; comLocal.setIdentity(); comLocal.block(0,3,3,1) = -com; // worldPose -> local visualization frame /*Eigen::Matrix4f localPose = sceneObject->getGlobalPoseVisualization().inverse() * worldPose; // com as matrix4f Eigen::Matrix4f comLocal; comLocal.setIdentity(); comLocal.block(0,3,3,1) = -com; //Eigen::Matrix4f localPoseAdjusted = localPose * comLocal; //Eigen::Matrix4f localPoseAdjusted = comLocal * localPose; //Eigen::Matrix4f localPoseAdjusted = localPose; //localPoseAdjusted.block(0,3,3,1) -= com; Eigen::Matrix4f comTrafo = Eigen::Matrix4f::Identity(); comTrafo.block(0,3,3,1) = -com; Eigen::Matrix4f localPoseAdjusted = localPose * comTrafo; //localPoseAdjusted.block(0,3,3,1) -= com; Eigen::Matrix4f resPose = sceneObject->getGlobalPoseVisualization() * localPoseAdjusted; */ Eigen::Matrix4f resPose = worldPose * comLocal; if (robotNodeActuator) { // get joint angle RobotNodePtr rn = robotNodeActuator->getRobotNode(); DynamicsWorldPtr w = DynamicsWorld::GetWorld(); DynamicsRobotPtr dr = w->getEngine()->getRobot(rn->getRobot()); BulletRobotPtr bdr = boost::dynamic_pointer_cast<BulletRobot>(dr); if (bdr) { // check if robotnode is connected with a joint std::vector<BulletRobot::LinkInfo> links = bdr->getLinks(rn); // update all involved joint values for (size_t i=0;i<links.size();i++) { if (links[i].nodeJoint) { float ja = bdr->getJointAngle(links[i].nodeJoint); // we can update the joint value via an RobotNodeActuator RobotNodeActuatorPtr rna (new RobotNodeActuator(links[i].nodeJoint)); rna->updateJointAngle(ja); } } // we assume that all models are handled by Bullet, so we do not need to update children robotNodeActuator->updateVisualizationPose(resPose, false); #if 0 if (rn->getName() == "Shoulder 1 L") { cout << "Shoulder 1 L:" << ja << ", speed:" << bdr->getJointSpeed(rn) << endl; } #endif } /*else { VR_WARNING << "Could not determine dynamic robot?!" << endl; }*/ } else { sceneObject->setGlobalPose(resPose); } }