void DynamicsRobot::createDynamicsNode( VirtualRobot::RobotNodePtr node )
{
VR_ASSERT(node);
// check if already created
if (hasDynamicsRobotNode(node))
return;
DynamicsWorldPtr dw = DynamicsWorld::GetWorld();
DynamicsObjectPtr drn = dw->CreateDynamicsObject(node);
VR_ASSERT(drn);
dynamicRobotNodes[node] = drn;
}
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);
}
}
