diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp
index f0b9af13ad453abac5e1148c779bc2c5c789cbf7..efa423e3adb77feb7e22d21fd4acfbd00114de5b 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp
@@ -19,6 +19,7 @@ namespace SimDynamics
BulletCoinQtViewer::BulletCoinQtViewer(DynamicsWorldPtr world)
{
bulletMaxSubSteps = 10;
+ enablePhysicsUpdates = true;
const float TIMER_MS = 30.0f;
@@ -63,7 +64,9 @@ void BulletCoinQtViewer::selectionCB( void *userdata, SoPath *path )
BulletCoinQtViewer *bulletViewer = static_cast<BulletCoinQtViewer*>(userdata);
VR_ASSERT(bulletViewer);
- bulletViewer->customSelection(path);
+ VR_INFO << "Selected object" << endl;
+
+ bulletViewer->customSelection(path);
bulletViewer->scheduleRedraw();
}
@@ -72,6 +75,8 @@ void BulletCoinQtViewer::deselectionCB( void *userdata, SoPath *path )
BulletCoinQtViewer *bulletViewer = static_cast<BulletCoinQtViewer*>(userdata);
VR_ASSERT(bulletViewer);
+ VR_INFO << "Deselected object" << endl;
+
bulletViewer->customDeselection(path);
bulletViewer->scheduleRedraw();
@@ -83,7 +88,7 @@ void BulletCoinQtViewer::timerCB(void * data, SoSensor * sensor)
VR_ASSERT(bulletViewer);
// now its safe to update physical information and set the models to the according poses
- bulletViewer->stepPhysics();
+ bulletViewer->updatePhysics();
// perform some custom updates if needed
bulletViewer->customUpdate();
@@ -115,6 +120,7 @@ void BulletCoinQtViewer::initSceneGraph( QFrame* embedViewer, SoNode* scene )
}
if (scene)
sceneGraph->addChild(scene);
+
viewer->setSceneGraph(sceneGraphRoot);
viewer->viewAll();
}
@@ -126,6 +132,8 @@ void BulletCoinQtViewer::scheduleRedraw()
void BulletCoinQtViewer::stepPhysics()
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+
//simple dynamics world doesn't handle fixed-time-stepping
float ms = getDeltaTimeMicroseconds();
@@ -140,7 +148,7 @@ void BulletCoinQtViewer::stepPhysics()
bulletEngine->activateAllObjects(); // avoid sleeping objects
updateMotors(dt1);
- bulletEngine->getBulletWorld()->stepSimulation(dt1,bulletMaxSubSteps);
+ bulletEngine->stepSimulation(dt1,bulletMaxSubSteps);
//optional but useful: debug drawing
//m_dynamicsWorld->debugDrawWorld();
@@ -174,6 +182,7 @@ void BulletCoinQtViewer::viewAll()
void BulletCoinQtViewer::addVisualization(DynamicsObjectPtr o, VirtualRobot::SceneObject::VisualizationType visuType)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
VR_ASSERT(o);
SceneObjectPtr so = o->getSceneObject();
VR_ASSERT(so);
@@ -182,12 +191,15 @@ void BulletCoinQtViewer::addVisualization(DynamicsObjectPtr o, VirtualRobot::Sce
if (n)
{
sceneGraph->addChild(n);
+ sceneGraph->addSelectionCallback( selectionCB, this );
+ sceneGraph->addDeselectionCallback( deselectionCB, this );
addedVisualizations[o] = n;
}
}
void BulletCoinQtViewer::addVisualization(DynamicsRobotPtr r, VirtualRobot::SceneObject::VisualizationType visuType)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
VR_ASSERT(r);
RobotPtr ro = r->getRobot();
VR_ASSERT(ro);
@@ -219,16 +231,18 @@ void BulletCoinQtViewer::addVisualization(DynamicsRobotPtr r, VirtualRobot::Scen
void BulletCoinQtViewer::removeVisualization( DynamicsObjectPtr o )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
VR_ASSERT(o);
if (addedVisualizations.find(o) != addedVisualizations.end())
{
sceneGraph->removeChild(addedVisualizations[o]);
- addedVisualizations.erase(o);
+ addedVisualizations.erase(o);
}
}
void BulletCoinQtViewer::removeVisualization( DynamicsRobotPtr r )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
VR_ASSERT(r);
if (addedRobotVisualizations.find(r) != addedRobotVisualizations.end())
{
@@ -239,7 +253,8 @@ void BulletCoinQtViewer::removeVisualization( DynamicsRobotPtr r )
void BulletCoinQtViewer::stopCB()
{
- if (timerSensor)
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ if (timerSensor)
{
SoSensorManager *sensor_mgr = SoDB::getSensorManager();
sensor_mgr->removeTimerSensor(timerSensor);
@@ -255,9 +270,35 @@ void BulletCoinQtViewer::stopCB()
void BulletCoinQtViewer::setBulletSimMaxSubSteps(int n)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
VR_ASSERT(n>=0);
bulletMaxSubSteps = n;
}
+bool BulletCoinQtViewer::engineRunning()
+{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ return enablePhysicsUpdates;
+}
+
+void BulletCoinQtViewer::stopEngine()
+{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ enablePhysicsUpdates = false;
+}
+void BulletCoinQtViewer::startEngine()
+{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ enablePhysicsUpdates = true;
+}
+
+void BulletCoinQtViewer::updatePhysics()
+{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+
+ if (enablePhysicsUpdates)
+ stepPhysics();
+}
+
}
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.h
index bec08ecc0c9e9886bc148d29382846a84fed0491..9085fde04304e1205ea7eb766c2aafef7fffcafe 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.h
@@ -40,6 +40,8 @@
#include <QtGui/QtGui>
#include <QtCore/QtCore>
+#include <boost/thread/recursive_mutex.hpp>
+
namespace SimDynamics
{
@@ -59,18 +61,30 @@ public:
void viewAll();
/*!
- Visualize the dynamics object.
+ Visualize dynamics object.
*/
void addVisualization(DynamicsObjectPtr o, VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full);
void addVisualization(DynamicsRobotPtr r, VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full);
/*!
- Remove the visualization of the dynamics object.
+ Remove visualization of dynamics object.
*/
void removeVisualization(DynamicsObjectPtr o);
void removeVisualization(DynamicsRobotPtr r);
- //! Stop callbacks
+ //! Returns true, if physics engine is running. False, if paused.
+ bool engineRunning();
+
+ //! Pauses the physics engine.
+ void stopEngine();
+
+ //! Restarts the engine
+ void startEngine();
+
+ //! Only allowed when engine is paused.
+ virtual void stepPhysics();
+
+ //! Stop callbacks which update the dynamics engine. Shuts down automatic physics engine functionality!
void stopCB();
/*!
@@ -81,6 +95,8 @@ public:
protected:
+ //checks if physics engine is enabled and performes a time step.
+ virtual void updatePhysics();
/*!
This method is called periodically, triggered by a timer callback.
@@ -109,7 +125,7 @@ protected:
btScalar getDeltaTimeMicroseconds();
void updateMotors(float dt);
- virtual void stepPhysics();
+
static void timerCB(void * data, SoSensor * sensor);
static void selectionCB( void *userdata, SoPath *path );
@@ -128,6 +144,10 @@ protected:
SoSelection* sceneGraph;
int bulletMaxSubSteps;
+
+ bool enablePhysicsUpdates;
+
+ boost::recursive_mutex engineMutex;
};
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
index bcee4c2730ac59332a4eefccca03ab70255a89a3..0e5fbfedf9ee9ee5175ac688cf6d60f2b0b6a0e7 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
@@ -5,7 +5,6 @@
#include <VirtualRobot/Obstacle.h>
#include <VirtualRobot/MathTools.h>
-
//#define DEBUG_FIXED_OBJECTS
namespace SimDynamics {
@@ -17,7 +16,7 @@ BulletEngine::BulletEngine()
overlappingPairCache = NULL;
constraintSolver = NULL;
dynamicsWorld = NULL;
- bulletRestitution = btScalar(0.1f);
+ bulletRestitution = btScalar(0);
bulletFriction = btScalar(0.5f);
bulletDampingLinear = btScalar(0.05f);
//bulletDampingAngular = btScalar(0.85f);
@@ -34,6 +33,7 @@ BulletEngine::~BulletEngine()
bool BulletEngine::init(const DynamicsWorldInfo &info)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
DynamicsEngine::init(info);
// Setup the bullet world
@@ -68,6 +68,7 @@ bool BulletEngine::init(const DynamicsWorldInfo &info)
bool BulletEngine::cleanup()
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
while (robots.size()>0)
{
size_t start = robots.size();
@@ -99,6 +100,7 @@ bool BulletEngine::cleanup()
bool BulletEngine::addObject( DynamicsObjectPtr o )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
BulletObjectPtr btObject = boost::dynamic_pointer_cast<BulletObject>(o);
if (!btObject)
{
@@ -137,6 +139,7 @@ bool BulletEngine::addObject( DynamicsObjectPtr o )
bool BulletEngine::removeObject( DynamicsObjectPtr o )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
BulletObjectPtr btObject = boost::dynamic_pointer_cast<BulletObject>(o);
if (!btObject)
{
@@ -150,6 +153,7 @@ bool BulletEngine::removeObject( DynamicsObjectPtr o )
bool BulletEngine::removeLink( BulletRobot::LinkInfo &l )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
dynamicsWorld->removeConstraint(l.joint.get());
this->resetCollisions(static_cast<DynamicsObject*>(l.dynNode1.get()));
this->resetCollisions(static_cast<DynamicsObject*>(l.dynNode2.get()));
@@ -163,6 +167,7 @@ btDynamicsWorld* BulletEngine::getBulletWorld()
void BulletEngine::createFloorPlane( const Eigen::Vector3f &pos, const Eigen::Vector3f &up )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
DynamicsEngine::createFloorPlane(pos,up);
float size = 100000.0f; // mm
float sizeSmall = 1000.0f;
@@ -193,17 +198,37 @@ void BulletEngine::createFloorPlane( const Eigen::Vector3f &pos, const Eigen::Ve
addObject(groundObjectBt);
}
+
+btMatrix3x3 BulletEngine::getRotMatrix(const Eigen::Matrix4f &pose)
+{
+ btMatrix3x3 rot(pose(0,0), pose(0,1), pose(0,2),
+ pose(1,0), pose(1,1), pose(1,2),
+ pose(2,0), pose(2,1), pose(2,2));
+ return rot;
+}
+
+Eigen::Matrix4f BulletEngine::getRotMatrix(const btMatrix3x3 &pose)
+{
+ Eigen::Matrix4f rot = Eigen::Matrix4f::Identity();
+
+ for (int a=0;a<3;a++)
+ for (int b=0;b<3;b++)
+ rot(a,b) = pose[a][b];
+ return rot;
+}
+
btTransform BulletEngine::getPoseBullet( const Eigen::Matrix4f &pose, bool scaling )
{
btTransform res;
float sc = 1.0f;
if (scaling && DynamicsWorld::convertMM2M)
sc = 0.001f; // mm -> m
- VirtualRobot::MathTools::Quaternion q = VirtualRobot::MathTools::eigen4f2quat(pose);
btVector3 pos(pose(0,3)*sc,pose(1,3)*sc,pose(2,3)*sc);
- btQuaternion rot(q.x,q.y,q.z,q.w);
- res.setOrigin(pos);
- res.setRotation(rot);
+ res.setOrigin(pos);
+ btMatrix3x3 rot = getRotMatrix(pose);
+ //VirtualRobot::MathTools::Quaternion q = VirtualRobot::MathTools::eigen4f2quat(pose);
+ //btQuaternion rot(q.x,q.y,q.z,q.w);
+ res.setBasis(rot);
return res;
}
@@ -213,13 +238,14 @@ Eigen::Matrix4f BulletEngine::getPoseEigen( const btTransform &pose, bool scalin
if (scaling && DynamicsWorld::convertMM2M)
sc = 1000.0f; // m -> mm
- btQuaternion q = pose.getRotation();
+ /*btQuaternion q = pose.getRotation();
VirtualRobot::MathTools::Quaternion qvr;
qvr.x = q.getX();
qvr.y = q.getY();
qvr.z = q.getZ();
qvr.w = q.getW();
- Eigen::Matrix4f res = VirtualRobot::MathTools::quat2eigen4f(qvr);
+ Eigen::Matrix4f res = VirtualRobot::MathTools::quat2eigen4f(qvr);*/
+ Eigen::Matrix4f res = getRotMatrix(pose.getBasis());
res(0,3) = pose.getOrigin().getX()*sc;
res(1,3) = pose.getOrigin().getY()*sc;
res(2,3) = pose.getOrigin().getZ()*sc;
@@ -252,6 +278,7 @@ Eigen::Vector3f BulletEngine::getVecEigen( const btVector3 &vec, bool scaling )
bool BulletEngine::addRobot( DynamicsRobotPtr r )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
BulletRobotPtr btRobot = boost::dynamic_pointer_cast<BulletRobot>(r);
if (!btRobot)
{
@@ -276,6 +303,7 @@ bool BulletEngine::addRobot( DynamicsRobotPtr r )
bool BulletEngine::removeRobot( DynamicsRobotPtr r )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
BulletRobotPtr btRobot = boost::dynamic_pointer_cast<BulletRobot>(r);
if (!btRobot)
{
@@ -300,6 +328,7 @@ bool BulletEngine::removeRobot( DynamicsRobotPtr r )
bool BulletEngine::addLink( BulletRobot::LinkInfo &l )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
#ifdef DEBUG_FIXED_OBJECTS
cout << "TEST2" << endl;
#else
@@ -314,6 +343,7 @@ bool BulletEngine::addLink( BulletRobot::LinkInfo &l )
void BulletEngine::print()
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
cout << "------------------ Bullet Engine ------------------" << endl;
for (size_t i=0;i<objects.size();i++)
{
@@ -339,14 +369,26 @@ void BulletEngine::print()
cout << "++++ Link " << j << ":" << links[j].node2->getName();
cout << " enabled:" << links[j].joint->isEnabled() << endl;
- boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(links[j].joint);
- if (hinge)
- {
- cout << " hinge motor enabled:" << hinge->getEnableAngularMotor() << endl;
- cout << " hinge angle :" << hinge->getHingeAngle() << endl;
- cout << " hinge max motor impulse :" << hinge->getMaxMotorImpulse() << endl;
- cout << " hinge motor target vel :" << hinge->getMotorTargetVelosity() << endl;
- }
+ boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(links[j].joint);
+ if (hinge)
+ {
+ cout << " hinge motor enabled:" << hinge->getEnableAngularMotor() << endl;
+ cout << " hinge angle :" << hinge->getHingeAngle() << endl;
+ cout << " hinge max motor impulse :" << hinge->getMaxMotorImpulse() << endl;
+ cout << " hinge motor target vel :" << hinge->getMotorTargetVelosity() << endl;
+ }
+ boost::shared_ptr<btGeneric6DofConstraint> dof = boost::dynamic_pointer_cast<btGeneric6DofConstraint>(links[j].joint);
+ if (dof)
+ {
+ btRotationalLimitMotor *m = dof->getRotationalLimitMotor(2);
+ VR_ASSERT(m);
+ cout << " generic_6DOF_joint: axis 5 (z)" << endl;
+ cout << " generic_6DOF_joint motor enabled:" << m->m_enableMotor << endl;
+ cout << " generic_6DOF_joint angle :" << m->m_currentPosition << endl;
+ cout << " generic_6DOF_joint max motor force :" << m->m_maxMotorForce << endl;
+ cout << " higeneric_6DOF_jointnge motor target vel :" << m->m_targetVelocity << endl;
+ }
+
}
}
@@ -355,6 +397,7 @@ void BulletEngine::print()
void BulletEngine::activateAllObjects()
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
for (size_t i=0;i<objects.size();i++)
{
BulletObjectPtr bo = boost::dynamic_pointer_cast<BulletObject>(objects[i]);
@@ -368,6 +411,7 @@ void BulletEngine::activateAllObjects()
std::vector<DynamicsEngine::DynamicsContactInfo> BulletEngine::getContacts()
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
//Assume world->stepSimulation or world->performDiscreteCollisionDetection has been called
std::vector<DynamicsEngine::DynamicsContactInfo> result;
@@ -404,5 +448,11 @@ std::vector<DynamicsEngine::DynamicsContactInfo> BulletEngine::getContacts()
return result;
}
+void BulletEngine::stepSimulation( float dt, int subSteps )
+{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ dynamicsWorld->stepSimulation(dt,subSteps);
+}
+
} // namespace SimDynamics
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
index a12005645da9f1bc5a51877fe3a59f6c90a915d9..fb7a9569162a7b8c4463480edf0dbd6eaddc6a03 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
@@ -65,6 +65,8 @@ public:
virtual void createFloorPlane(const Eigen::Vector3f &pos, const Eigen::Vector3f &up);
+ virtual void stepSimulation(float dt, int subSteps);
+
btDynamicsWorld* getBulletWorld();
/*!
@@ -87,7 +89,8 @@ public:
static Eigen::Matrix4f getPoseEigen( const btTransform &pose, bool scaling = true );
static btVector3 getVecBullet( const Eigen::Vector3f &vec, bool scaling = true );
static Eigen::Vector3f getVecEigen( const btVector3 &vec, bool scaling = true );
-
+ static btMatrix3x3 getRotMatrix(const Eigen::Matrix4f &pose);
+ static Eigen::Matrix4f getRotMatrix(const btMatrix3x3 &pose);
protected:
class CustomCollisionCallback : public btOverlapFilterCallback
@@ -116,8 +119,7 @@ protected:
virtual bool addLink(BulletRobot::LinkInfo &l);
virtual bool removeLink(BulletRobot::LinkInfo &l);
-
-
+
btDynamicsWorld *dynamicsWorld;
btBroadphaseInterface* overlappingPairCache;
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
index 127727938d306512fd2e1f364104a9dd6c247b94..23374714ac57cac982c1be9239aa4702fa45fb4f 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
@@ -12,6 +12,7 @@
#include <BulletCollision/CollisionShapes/btShapeHull.h>
//#define DEBUG_FIXED_OBJECTS
+//#define USE_BULLET_GENERIC_6DOF_CONSTRAINT
using namespace VirtualRobot;
@@ -21,6 +22,10 @@ BulletObject::BulletObject(VirtualRobot::SceneObjectPtr o, SimulationType type)
: DynamicsObject(o, type)
{
float interatiaFactor = 5.0f;
+#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
+ interatiaFactor = 5.0f;
+#endif
+
btMargin=(btScalar)(0.000001);
com.setZero();
THROW_VR_EXCEPTION_IF(!o,"NULL object");
@@ -54,6 +59,10 @@ BulletObject::BulletObject(VirtualRobot::SceneObjectPtr o, SimulationType type)
{
//THROW_VR_EXCEPTION ("mass == 0 -> SimulationType must not be eDynamic! ");
mass = btScalar(1.0f); // give object a dummy mass
+#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
+ mass = btScalar(1.0f);
+#endif
+
//type = eKinematic;
if (colModel)
{
@@ -73,18 +82,24 @@ BulletObject::BulletObject(VirtualRobot::SceneObjectPtr o, SimulationType type)
if (colModel)
{
collisionShape->calculateLocalInertia(mass,localInertia);
+//#ifndef USE_BULLET_GENERIC_6DOF_CONSTRAINT
// check for small values
if (localInertia.length()<1.0f && localInertia.length()>0)
localInertia /= localInertia.length(); // small inertia values result in freaking out joints ?!
+//#endif
} else
+#ifndef USE_BULLET_GENERIC_6DOF_CONSTRAINT
localInertia.setValue(btScalar(1),btScalar(1),btScalar(1)); // give Object a dummy inertia matrix
- //localInertia.setValue(btScalar(40),btScalar(40),btScalar(40)); // give Object a dummy inertia matrix (large values needed, otherwise the objects will not stay connected on strong impulses)
+#else
+ localInertia.setValue(btScalar(1),btScalar(1),btScalar(1)); // give Object a dummy inertia matrix
+#endif
+ //localInertia.setValue(btScalar(40),btScalar(40),btScalar(40)); // give Object a dummy inertia matrix (large values needed, otherwise the objects will not stay connected on strong impulses)
}
#endif
localInertia *= interatiaFactor;
motionState = new SimoxMotionState(o);
btRigidBody::btRigidBodyConstructionInfo btRBInfo(mass,motionState,collisionShape.get(),localInertia);
- btRBInfo.m_additionalDamping = true;
+ //btRBInfo.m_additionalDamping = true;
rigidBody.reset(new btRigidBody(btRBInfo));
rigidBody->setUserPointer((void*)(this));
@@ -205,5 +220,12 @@ void BulletObject::setAngularVelocity( const Eigen::Vector3f &vel )
rigidBody->setAngularVelocity(btVel);
}
+Eigen::Matrix4f BulletObject::getComGlobal()
+{
+ btTransform tr;
+ motionState->getWorldTransform(tr);
+ return BulletEngine::getPoseEigen(tr);
+}
+
} // namespace VirtualRobot
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
index 98407cdad2f0fa6b8cd7d01cad506fda9a5d25c6..e2417ac2a3215dc2bf7c768b7b00f5684b6b415f 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
@@ -65,6 +65,9 @@ public:
void setLinearVelocity(const Eigen::Vector3f &vel);
void setAngularVelocity(const Eigen::Vector3f &vel);
+
+ //! This is the world pose which is set by bullet
+ Eigen::Matrix4f getComGlobal();
protected:
btConvexHullShape* createConvexHullShape(VirtualRobot::TriMeshModelPtr trimesh);
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index 9aa6fcd39b2de5f48ac69b8ee3492d51d89fb983..5290b12c69ecdc0a8e36ac16942151e104ef2b2f 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -10,6 +10,10 @@
#include <VirtualRobot/Nodes/RobotNodeFixed.h>
#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
+// either hinge or generic6DOF constraints can be used
+//#define USE_BULLET_GENERIC_6DOF_CONSTRAINT
+
+
#include <boost/pointer_cast.hpp>
//#define DEBUG_FIXED_OBJECTS
@@ -21,7 +25,7 @@ namespace SimDynamics {
BulletRobot::BulletRobot(VirtualRobot::RobotPtr rob, bool enableJointMotors)
: DynamicsRobot(rob)
{
- bulletMaxMotorImulse = 5.0f;
+ bulletMaxMotorImulse = 10.0f;
buildBulletModels(enableJointMotors);
}
@@ -58,7 +62,7 @@ void BulletRobot::buildBulletModels(bool enableJointMotors)
std::vector<SceneObjectPtr> children = rn->getChildren();
for (size_t c=0;c<children.size();c++)
{
- // check what to to
+ // check what to do
/*
bool fixed = !(children[c]->isRotationalJoint() || children[c]->isTranslationalJoint());
if (fixed && !(children[c]->getCollisionModel()))
@@ -153,9 +157,11 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
{
VR_WARNING << "translational joint nyi, creating a fixed link..." << endl;
}
- bool createHinge = node2->isRotationalJoint();
- if (createHinge)
+ bool createJoint = node2->isRotationalJoint();
+ if (createJoint)
{
+
+ // create joint
boost::shared_ptr<RobotNodeRevolute> rnRev2 = boost::dynamic_pointer_cast<RobotNodeRevolute>(node2);
// transform axis direction (not position!)
@@ -187,22 +193,78 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
btVector3 pivot1 = BulletEngine::getVecBullet(anchor_inNode1.block(0,3,3,1));
btVector3 pivot2 = BulletEngine::getVecBullet(anchor_inNode2.block(0,3,3,1));
- btTransform pivTr1;
- pivTr1.setIdentity();
+
+
+ /*pivTr1.setIdentity();
pivTr1.setOrigin(pivot1);
btTransform pivTr2;
pivTr2.setIdentity();
pivTr2.setOrigin(pivot2);
btTransform pivotTest1 = btBody1->getWorldTransform()*pivTr1;
- btTransform pivotTest2 = btBody2->getWorldTransform()*pivTr2;
-
- btVector3 axis1 = BulletEngine::getVecBullet(axis_inLocal1,false);
- btVector3 axis2 = BulletEngine::getVecBullet(axis_inLocal2,false);
-
- boost::shared_ptr<btHingeConstraint> hinge(new btHingeConstraint(*btBody1, *btBody2, pivot1, pivot2, axis1, axis2,false));
- float limMin,limMax;
+ btTransform pivotTest2 = btBody2->getWorldTransform()*pivTr2;*/
+
+ float limMin,limMax;
limMin = node2->getJointLimitLo();
limMax = node2->getJointLimitHi();
+
+#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
+
+ MathTools::Quaternion axisRotQuat1 = MathTools::getRotation(axis_inLocal1,Eigen::Vector3f::UnitX());
+ MathTools::Quaternion axisRotQuat2 = MathTools::getRotation(axis_inLocal2,Eigen::Vector3f::UnitX());
+ Eigen::Matrix4f axisRot1 = MathTools::quat2eigen4f(axisRotQuat1);
+ Eigen::Matrix4f axisRot2 = MathTools::quat2eigen4f(axisRotQuat2);
+ float ang1,ang2;
+ Eigen::Vector3f ax1,ax2;
+ // test
+ MathTools::eigen4f2axisangle(axisRot1,ax1,ang1);
+ MathTools::eigen4f2axisangle(axisRot2,ax2,ang2);
+
+ Eigen::Matrix4f anchorAxisXAligned1;// = anchor_inNode1 * axisRot1;
+ Eigen::Matrix4f anchorAxisXAligned2 = anchor_inNode2 * axisRot2;
+
+ // we have to express anchorAxisXAligned2 in node1 coord system (only rotation!)
+ Eigen::Matrix4f anchorAxisXAligned2_global = tmpGp2 * anchorAxisXAligned2;
+ Eigen::Matrix4f anchorAxisXAligned2_local1 = tmpGp1.inverse() * anchorAxisXAligned2_global;
+ anchorAxisXAligned1 = anchorAxisXAligned2_local1;
+ // re-set position
+ anchorAxisXAligned1.block(0,3,3,1) = anchor_inNode1.block(0,3,3,1);
+
+ btTransform pivTr1 = BulletEngine::getPoseBullet(anchorAxisXAligned1);
+ btTransform pivTr2 = BulletEngine::getPoseBullet(anchorAxisXAligned2);
+ boost::shared_ptr<btGeneric6DofConstraint> dof(new btGeneric6DofConstraint(*btBody1, *btBody2, pivTr1, pivTr2, true));
+ btRotationalLimitMotor *m = dof->getRotationalLimitMotor(0);
+ VR_ASSERT(m);
+ m->m_targetVelocity = 0;
+ m->m_currentPosition = node2->getJointValue();
+ /*btScalar startAngle = node2->getJointValue();
+ btScalar startAngleBT = dof->getRotationalLimitMotor(2)->m_currentPosition;
+ btScalar limMinBT, limMaxBT;
+ btScalar diff = (startAngleBT + startAngle);
+ limMinBT = diff - limMax;// limMin + diff;//
+ limMaxBT = diff - limMin;// limMax + diff;//
+ if (fabs(startAngleBT - startAngle)>1e-6)
+ {
+ cout << "joint " << node2->getName() << ": jv diff:" << diff << endl;
+ cout << "Simox limits: " << limMin << "/" << limMax << ", bullet limits:" << limMinBT << "/" << limMaxBT << endl;
+ }*/
+ dof->setLimit(0,0,0);
+ dof->setLimit(1,0,0);
+ dof->setLimit(2,0,0);
+ dof->setLimit(3,btScalar(limMin),btScalar(limMax));
+ //dof->setLimit(3,-btScalar(limMax),-btScalar(limMin)); // inverted joint direction
+ dof->setLimit(4,0,0);
+ dof->setLimit(5,0,0);
+
+ //dof->setLimit(5,btScalar(limMin),btScalar(limMax));
+ vr2bulletOffset = 0;//diff;
+ //hinge->setLimit(btScalar(limMin),btScalar(limMax));
+ //hinge->setAngularOnly(true);
+ joint = dof;
+#else
+ btVector3 axis1 = BulletEngine::getVecBullet(axis_inLocal1,false);
+ btVector3 axis2 = BulletEngine::getVecBullet(axis_inLocal2,false);
+ boost::shared_ptr<btHingeConstraint> hinge(new btHingeConstraint(*btBody1, *btBody2, pivot1, pivot2, axis1, axis2,false));
+
//hinge->setParam(BT_CONSTRAINT_STOP_ERP,0.9f);
/*
hinge->setParam(BT_CONSTRAINT_CFM,0.9f);
@@ -213,9 +275,9 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
btScalar startAngle = node2->getJointValue();
btScalar startAngleBT = hinge->getHingeAngle();
btScalar limMinBT, limMaxBT;
- btScalar diff = (startAngleBT + startAngle);
- limMinBT = diff - limMax;// limMin + diff;//
- limMaxBT = diff - limMin;// limMax + diff;//
+ btScalar diff = 0;//(startAngleBT + startAngle);
+ limMinBT = limMin + diff;//diff - limMax;//
+ limMaxBT = limMax + diff;//diff - limMin;//
if (fabs(startAngleBT - startAngle)>1e-6)
{
cout << "joint " << node2->getName() << ": jv diff:" << diff << endl;
@@ -226,6 +288,8 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
//hinge->setLimit(btScalar(limMin),btScalar(limMax));
//hinge->setAngularOnly(true);
joint = hinge;
+
+#endif
} else /*if (node2->isTranslationalJoint())
{
@@ -286,9 +350,18 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
//localA.setOrigin(pivot1);
//localB.setOrigin(pivot2);
boost::shared_ptr<btGeneric6DofConstraint> generic6Dof(new btGeneric6DofConstraint(*btBody1, *btBody2, localA, localB, true));
- btVector3 limitZero(0,0,0);
+
+ generic6Dof->setOverrideNumSolverIterations(100);
+
+ //float totalMass = 1.f/btBody1->getInvMass() + 1.f/btBody2->getInvMass();
+
+ //generic6Dof->setBreakingImpulseThreshold(2*totalMass);//needed? copied from voronoi demo
+
+ for (int i=0;i<6;i++)
+ generic6Dof->setLimit(i,0,0);
+ /*btVector3 limitZero(0,0,0);
generic6Dof->setAngularLowerLimit(limitZero);
- generic6Dof->setAngularUpperLimit(limitZero);
+ generic6Dof->setAngularUpperLimit(limitZero);*/
/*generic6Dof->setParam(BT_CONSTRAINT_STOP_CFM,0.01f,0);
generic6Dof->setParam(BT_CONSTRAINT_STOP_CFM,0.01f,1);
@@ -321,10 +394,10 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
// check if node1 owns a 3d model or a parent
// -> disable according model, so it won't be considered for collision detection during simulation
- if (!node1->getCollisionModel())
- {
- RobotNodePtr parent = boost::dynamic_pointer_cast<RobotNode>(node1->getParent());
- while (parent)
+ //if (!node1->getCollisionModel())
+ {
+ RobotNodePtr parent = boost::dynamic_pointer_cast<RobotNode>(node1);
+ while (parent)
{
if (parent->getCollisionModel())
{
@@ -401,14 +474,34 @@ void BulletRobot::actuateJoints(float dt)
if (it->second.node->isRotationalJoint())
{
LinkInfo link = getLink(it->second.node);
+#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
+ boost::shared_ptr<btGeneric6DofConstraint> dof = boost::dynamic_pointer_cast<btGeneric6DofConstraint>(link.joint);
+ VR_ASSERT(dof);
+ btRotationalLimitMotor *m = dof->getRotationalLimitMotor(0);
+ VR_ASSERT(m);
+ if (it->second.enabled)
+ {
+ btScalar targ = btScalar(it->second.jointValueTarget+link.jointValueOffset);
+ //btScalar act = btScalar(it->first->getJointValue());
+ btScalar act = btScalar(getJointAngle(it->first));
+ m->m_enableMotor = true;
+ m->m_targetVelocity = (targ-act); // inverted joint dir?!
+ } else
+ m->m_enableMotor = false;
+#else
boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
VR_ASSERT(hinge);
if (it->second.enabled)
{
- hinge->enableMotor(true);
- hinge->setMotorTarget(it->second.jointValueTarget+link.jointValueOffset,dt);
+ btScalar targ = btScalar(it->second.jointValueTarget+link.jointValueOffset);
+ //btScalar act = btScalar(it->first->getJointValue());
+ btScalar act = btScalar(getJointAngle(it->first));
+ hinge->enableAngularMotor(true,(targ-act),10.0f);
+ //hinge->enableMotor(true);
+ //hinge->setMotorTarget(it->second.jointValueTarget+link.jointValueOffset,dt);
} else
hinge->enableMotor(false);
+#endif
}
it++;
@@ -442,11 +535,22 @@ void BulletRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointValue
if (node->isRotationalJoint())
{
LinkInfo link = getLink(node);
+#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
+ boost::shared_ptr<btGeneric6DofConstraint> dof = boost::dynamic_pointer_cast<btGeneric6DofConstraint>(link.joint);
+ VR_ASSERT(dof);
+ btRotationalLimitMotor *m = dof->getRotationalLimitMotor(0);
+ VR_ASSERT(m);
+ m->m_enableMotor = true;
+ m->m_maxMotorForce = 5;//bulletMaxMotorImulse; //?!
+ m->m_maxLimitForce = 300;
+ DynamicsRobot::actuateNode(node,jointValue);
+#else
boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
VR_ASSERT(hinge);
hinge->enableAngularMotor(true,0.0f,bulletMaxMotorImulse);// is max impulse ok?! (10 seems to be ok, 1 oscillates)
- DynamicsRobot::actuateNode(node,jointValue); // inverted joint direction in bullet
- } else
+ DynamicsRobot::actuateNode(node,jointValue); // inverted joint direction in bullet
+#endif
+ } else
{
VR_ERROR << "Only Revolute joints implemented so far..." << endl;
}
@@ -461,13 +565,28 @@ float BulletRobot::getJointAngle( VirtualRobot::RobotNodePtr rn )
return 0.0f;
}
LinkInfo link = getLink(rn);
+#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
+ boost::shared_ptr<btGeneric6DofConstraint> dof = boost::dynamic_pointer_cast<btGeneric6DofConstraint>(link.joint);
+ VR_ASSERT(dof);
+ btRotationalLimitMotor *m = dof->getRotationalLimitMotor(0);
+ VR_ASSERT(m);
+ dof->calculateTransforms();
+ float a1 = dof->getAngle(0);
+ float a2 = m->m_currentPosition;
+ if (fabs(a1-a2)>0.05f)
+ {
+ VR_INFO << "Angle diff " << a1 << ", " << a2 << endl;
+ }
+ return (a2-link.jointValueOffset);// inverted joint direction in bullet
+#else
boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
if (!hinge)
{
- VR_WARNING << "RobotNode " << rn->getName() << " is not associated with a hinge joint?!" << endl;
+ //VR_WARNING << "RobotNode " << rn->getName() << " is not associated with a hinge joint?!" << endl;
return 0.0f;
}
- return (hinge->getHingeAngle()-link.jointValueOffset);// inverted joint direction in bullet
+ return (hinge->getHingeAngle()-link.jointValueOffset);// inverted joint direction in bullet
+#endif
}
float BulletRobot::getJointSpeed( VirtualRobot::RobotNodePtr rn )
@@ -479,18 +598,42 @@ float BulletRobot::getJointSpeed( VirtualRobot::RobotNodePtr rn )
return 0.0f;
}
LinkInfo link = getLink(rn);
+#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
+ boost::shared_ptr<btGeneric6DofConstraint> dof = boost::dynamic_pointer_cast<btGeneric6DofConstraint>(link.joint);
+ VR_ASSERT(dof);
+ btRotationalLimitMotor *m = dof->getRotationalLimitMotor(0);
+ VR_ASSERT(m);
+ return m->m_targetVelocity;
+#else
boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
if (!hinge)
{
VR_WARNING << "RobotNode " << rn->getName() << " is not associated with a hinge joint?!" << endl;
return 0.0f;
}
- return hinge->getMotorTargetVelosity();// inverted joint direction in bullet
+ return hinge->getMotorTargetVelosity();
+#endif
}
float BulletRobot::getNodeTarget( VirtualRobot::RobotNodePtr node )
{
- return DynamicsRobot::getNodeTarget(node);// inverted joint direction in bullet
+#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
+ return DynamicsRobot::getNodeTarget(node);
+#else
+ return DynamicsRobot::getNodeTarget(node);
+#endif
+
+}
+
+Eigen::Matrix4f BulletRobot::getComGlobal( VirtualRobot::RobotNodePtr rn )
+{
+ BulletObjectPtr bo = boost::dynamic_pointer_cast<BulletObject>(getDynamicsRobotNode(rn));
+ if (!bo)
+ {
+ VR_ERROR << "Could not cast object..." << endl;
+ return Eigen::Matrix4f::Identity();
+ }
+ return bo->getComGlobal();
}
} // namespace VirtualRobot
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
index 484073ccc7022944f699118f27228659246d0bac..5f68a801e44a38bdb34ddfc3d5442715765a2c8c 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
@@ -82,6 +82,11 @@ public:
virtual float getJointSpeed(VirtualRobot::RobotNodePtr rn);
virtual float getNodeTarget(VirtualRobot::RobotNodePtr node);
+ /*!
+ Returns the CoM pose, which is reported by bullet
+ */
+ virtual Eigen::Matrix4f getComGlobal(VirtualRobot::RobotNodePtr rn);
+
// experimental...
virtual void ensureKinematicConstraints();
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.cpp b/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.cpp
index 490b15d72ea0529720d48b70e4fd09d800fc0bd3..6d9b6fa2538267cd22e49233e1e0217170f64b59 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.cpp
@@ -18,7 +18,7 @@ SimoxCollisionDispatcher::~SimoxCollisionDispatcher()
{
}
-bool SimoxCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionObject* body1)
+bool SimoxCollisionDispatcher::needsCollision(const btCollisionObject* body0,const btCollisionObject* body1)
{
SimDynamics::BulletObject* o0 = static_cast<SimDynamics::BulletObject*>(body0->getUserPointer());
SimDynamics::BulletObject* o1 = static_cast<SimDynamics::BulletObject*>(body1->getUserPointer());
@@ -27,7 +27,7 @@ bool SimoxCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisi
return false;
}
-bool SimoxCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionObject* body1)
+bool SimoxCollisionDispatcher::needsResponse(const btCollisionObject* body0,const btCollisionObject* body1)
{
SimDynamics::BulletObject* o0 = static_cast<SimDynamics::BulletObject*>(body0->getUserPointer());
SimDynamics::BulletObject* o1 = static_cast<SimDynamics::BulletObject*>(body1->getUserPointer());
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.h b/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.h
index 1c6866e7f9b957c8825935738cb3631112c85eab..e4d9ec4a2a72819dc66e09bbcd2d9f0035c853a1 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.h
@@ -43,8 +43,8 @@ struct SIMDYNAMICS_IMPORT_EXPORT SimoxCollisionDispatcher : public btCollisionDi
{
SimoxCollisionDispatcher (BulletEngine* engine, btCollisionConfiguration* collisionConfiguration);
virtual ~SimoxCollisionDispatcher();
- virtual bool needsCollision(btCollisionObject* body0,btCollisionObject* body1);
- virtual bool needsResponse(btCollisionObject* body0,btCollisionObject* body1);
+ virtual bool needsCollision(const btCollisionObject* body0,const btCollisionObject* body1);
+ virtual bool needsResponse(const btCollisionObject* body0,const btCollisionObject* body1);
protected:
BulletEngine* engine;
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
index 47f972d750b59e21ca793779b3151295da1b1215..cb3b527312fac2210865e25c2995e11dd539e434 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
@@ -1,7 +1,9 @@
#include "SimoxMotionState.h"
#include "BulletEngine.h"
+#include "../../DynamicsWorld.h"
+#include <boost/pointer_cast.hpp>
using namespace VirtualRobot;
@@ -10,6 +12,7 @@ namespace SimDynamics
SimoxMotionState::SimoxMotionState( VirtualRobot::SceneObjectPtr sceneObject)
+ :btDefaultMotionState()
{
this->sceneObject = sceneObject;
initalGlobalPose.setIdentity();
@@ -60,6 +63,7 @@ void SimoxMotionState::setWorldTransform(const btTransform& worldPose)
m_graphicsWorldTrans = _transform; // this is used for debug drawing
_graphicsTransfrom = _transform;
//_graphicsTransfrom.getOrigin();// -= _comOffset.getOrigin(); // com adjusted
+
setGlobalPoseSimox( BulletEngine::getPoseEigen(_graphicsTransfrom) );
}
@@ -86,29 +90,41 @@ void SimoxMotionState::setGlobalPoseSimox( const Eigen::Matrix4f& worldPose )
comLocal.setIdentity();
comLocal.block(0,3,3,1) = -com;
- // apply com
- Eigen::Matrix4f localPoseAdjusted = comLocal * localPose;
+ //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;
- //Eigen::Matrix4f resPose = worldPose;
- // assuming we get the com adjusted pose
- //Eigen::Matrix4f gp = BulletEngine::getPoseEigen(worldPose);
- // Determine pose of simox model
- /*
- Eigen::Matrix4f comLocal = Eigen::Matrix4f::Identity();
- comLocal.block(0,3,3,1) = -com;
- Eigen::Matrix4f resPose = comLocal * gp;*/
-
if (robotNodeActuator)
{
- // we assume that all models are handled by Bullet, so we do not need to update children
- robotNodeActuator->updateVisualizationPose(resPose,false);
+ // 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)
+ {
+ float ja = 0;
+ if (bdr->hasLink(rn))
+ ja = bdr->getJointAngle(rn);
+
+ // we assume that all models are handled by Bullet, so we do not need to update children
+ robotNodeActuator->updateVisualizationPose(resPose, ja, false);
+ } else
+ {
+ VR_WARNING << "Could not determine dynamic robot?!" << endl;
+ }
} else
{
sceneObject->setGlobalPose(resPose);
@@ -118,7 +134,7 @@ void SimoxMotionState::setGlobalPoseSimox( const Eigen::Matrix4f& worldPose )
void SimoxMotionState::setGlobalPose( const Eigen::Matrix4f &pose )
{
initalGlobalPose = pose;
- /* conervt to local coord system, apply comoffset and convert back*/
+ /* convert to local coord system, apply comoffset and convert back*/
Eigen::Matrix4f poseLocal = sceneObject->getGlobalPoseVisualization().inverse() * pose;
poseLocal.block(0,3,3,1) += com;
Eigen::Matrix4f poseGlobal = sceneObject->getGlobalPoseVisualization() * poseLocal;
diff --git a/SimDynamics/DynamicsEngine/DynamicsEngine.cpp b/SimDynamics/DynamicsEngine/DynamicsEngine.cpp
index e9c95be7c99ba5d29b9c9c2926cb5b31e48c01f8..ab02017b5c9ed730175d0e6ee2c2efb0d8344476 100644
--- a/SimDynamics/DynamicsEngine/DynamicsEngine.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsEngine.cpp
@@ -18,6 +18,7 @@ DynamicsEngine::~DynamicsEngine()
Eigen::Vector3f DynamicsEngine::getGravity()
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
return dynamicsInfo.gravity;
}
@@ -29,6 +30,7 @@ bool DynamicsEngine::init( const DynamicsWorldInfo &info )
bool DynamicsEngine::addObject( DynamicsObjectPtr o )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
if (find(objects.begin(), objects.end(), o) == objects.end())
{
objects.push_back(o);
@@ -38,6 +40,7 @@ bool DynamicsEngine::addObject( DynamicsObjectPtr o )
bool DynamicsEngine::removeObject( DynamicsObjectPtr o )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
std::vector<DynamicsObjectPtr>::iterator it = find(objects.begin(), objects.end(), o);
if (it == objects.end())
return false;
@@ -48,14 +51,24 @@ bool DynamicsEngine::removeObject( DynamicsObjectPtr o )
void DynamicsEngine::createFloorPlane( const Eigen::Vector3f &pos, const Eigen::Vector3f &up )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
floorPos = pos;
floorUp = up;
}
bool DynamicsEngine::addRobot( DynamicsRobotPtr r )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
if (find(robots.begin(), robots.end(), r) == robots.end())
{
+ for (size_t i=0;i<robots.size();i++)
+ {
+ if (robots[i]->getRobot() == r->getRobot())
+ {
+ VR_ERROR << "Only one DynamicsWrapper per robot allowed. Robot " << r->getName() << endl;
+ return false;
+ }
+ }
robots.push_back(r);
}
return true;
@@ -63,6 +76,7 @@ bool DynamicsEngine::addRobot( DynamicsRobotPtr r )
bool DynamicsEngine::removeRobot( DynamicsRobotPtr r )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
std::vector<DynamicsRobotPtr>::iterator it = find(robots.begin(), robots.end(), r);
if (it == robots.end())
return false;
@@ -73,6 +87,7 @@ bool DynamicsEngine::removeRobot( DynamicsRobotPtr r )
void DynamicsEngine::disableCollision( DynamicsObject* o1, DynamicsObject* o2 )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
std::vector<DynamicsObject*>::iterator i1 = find(collisionDisabled[o1].begin(),collisionDisabled[o1].end(),o2);
if (i1==collisionDisabled[o1].end())
{
@@ -87,6 +102,7 @@ void DynamicsEngine::disableCollision( DynamicsObject* o1, DynamicsObject* o2 )
void DynamicsEngine::disableCollision( DynamicsObject* o1 )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
std::vector<DynamicsObject*>::iterator i1 = find(collisionToAllDisabled.begin(),collisionToAllDisabled.end(),o1);
if (i1 == collisionToAllDisabled.end())
collisionToAllDisabled.push_back(o1);
@@ -94,6 +110,7 @@ void DynamicsEngine::disableCollision( DynamicsObject* o1 )
void DynamicsEngine::enableCollision( DynamicsObject* o1, DynamicsObject* o2 )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
//if (o1 < o2)
//{
std::vector<DynamicsObject*>::iterator i1 = find(collisionDisabled[o1].begin(),collisionDisabled[o1].end(),o2);
@@ -113,6 +130,7 @@ void DynamicsEngine::enableCollision( DynamicsObject* o1, DynamicsObject* o2 )
void DynamicsEngine::enableCollision( DynamicsObject* o1 )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
std::vector<DynamicsObject*>::iterator i1 = find(collisionToAllDisabled.begin(),collisionToAllDisabled.end(),o1);
if (i1 != collisionToAllDisabled.end())
collisionToAllDisabled.erase(i1);
@@ -120,12 +138,14 @@ void DynamicsEngine::enableCollision( DynamicsObject* o1 )
bool DynamicsEngine::checkCollisionEnabled( DynamicsObject* o1 )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
std::vector<DynamicsObject*>::iterator i1 = find(collisionToAllDisabled.begin(),collisionToAllDisabled.end(),o1);
return (i1 == collisionToAllDisabled.end());
}
bool DynamicsEngine::checkCollisionEnabled( DynamicsObject* o1, DynamicsObject* o2 )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
if (!checkCollisionEnabled(o1))
return false;
if (!checkCollisionEnabled(o2))
@@ -147,6 +167,7 @@ bool DynamicsEngine::checkCollisionEnabled( DynamicsObject* o1, DynamicsObject*
void DynamicsEngine::resetCollisions( DynamicsObject* o )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
collisionDisabled.erase(o);
std::map < DynamicsObject*, std::vector<DynamicsObject*> >::iterator i1 = collisionDisabled.begin();
while (i1 != collisionDisabled.end())
@@ -163,18 +184,34 @@ void DynamicsEngine::resetCollisions( DynamicsObject* o )
std::vector<DynamicsRobotPtr> DynamicsEngine::getRobots()
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
return robots;
}
std::vector<DynamicsObjectPtr> DynamicsEngine::getObjects()
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
return objects;
}
std::vector<DynamicsEngine::DynamicsContactInfo> DynamicsEngine::getContacts()
{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
std::vector<DynamicsEngine::DynamicsContactInfo> result;
return result;
}
+SimDynamics::DynamicsRobotPtr DynamicsEngine::getRobot( VirtualRobot::RobotPtr r )
+{
+ boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ for (size_t i=0;i<robots.size();i++)
+ {
+ if (robots[i]->getRobot() == r)
+ {
+ return robots[i];
+ }
+ }
+ return DynamicsRobotPtr();
+}
+
} // namespace SimDynamics
diff --git a/SimDynamics/DynamicsEngine/DynamicsEngine.h b/SimDynamics/DynamicsEngine/DynamicsEngine.h
index 339cb34faca42ee5a5df7f88a5160ea39a54a391..ee010f2370d31c35fce62021ec7f1fbbc63590cf 100644
--- a/SimDynamics/DynamicsEngine/DynamicsEngine.h
+++ b/SimDynamics/DynamicsEngine/DynamicsEngine.h
@@ -26,6 +26,7 @@
#include "../SimDynamics.h"
#include "DynamicsObject.h"
#include "DynamicsRobot.h"
+#include <boost/thread/recursive_mutex.hpp>
namespace SimDynamics
{
@@ -123,6 +124,11 @@ public:
virtual std::vector<DynamicsEngine::DynamicsContactInfo> getContacts();
+ /*!
+ Returns the dynamics version of r.
+ An empty pointer is returned if no dynamic robot created from r has been added so far.
+ */
+ virtual DynamicsRobotPtr getRobot(VirtualRobot::RobotPtr r);
protected:
DynamicsWorldInfo dynamicsInfo;
@@ -136,6 +142,9 @@ protected:
Eigen::Vector3f floorPos;
Eigen::Vector3f floorUp;
+
+ boost::recursive_mutex engineMutex;
+
};
typedef boost::shared_ptr<DynamicsEngine> DynamicsEnginePtr;
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
index 60b3876b76e509cc604728766699fb2bda439d87..b4390815f70a762dce3ffa563a6b2900e09db587 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
@@ -145,6 +145,14 @@ float DynamicsRobot::getJointSpeed( VirtualRobot::RobotNodePtr rn )
return 0.0f;
}
+Eigen::Matrix4f DynamicsRobot::getComGlobal( VirtualRobot::RobotNodePtr rn )
+{
+ Eigen::Matrix4f com = Eigen::Matrix4f::Identity();
+ com.block(0,3,3,1) = rn->getCoMLocal();
+ com = rn->getGlobalPoseVisualization()*com;
+ return com;
+}
+
/*
void DynamicsRobot::setPose( const Eigen::Matrix4f &pose )
{
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.h b/SimDynamics/DynamicsEngine/DynamicsRobot.h
index 3cdf01c0dae4cf8edceec029c2fb4b61f928f35a..11b12eafebef1b117ffbeff9f803bc20635e2849 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.h
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.h
@@ -86,6 +86,8 @@ public:
virtual float getJointAngle(VirtualRobot::RobotNodePtr rn);
virtual float getJointSpeed(VirtualRobot::RobotNodePtr rn);
+ virtual Eigen::Matrix4f getComGlobal(VirtualRobot::RobotNodePtr rn);
+
protected:
virtual void createDynamicsNode(VirtualRobot::RobotNodePtr node);
diff --git a/SimDynamics/DynamicsWorld.cpp b/SimDynamics/DynamicsWorld.cpp
index 0911208917895123dae9f4ec35732ef4b503af83..ea1f622afd434fb8d2eaf162c86b4e78e7726016 100644
--- a/SimDynamics/DynamicsWorld.cpp
+++ b/SimDynamics/DynamicsWorld.cpp
@@ -123,7 +123,7 @@ SimDynamics::DynamicsRobotPtr DynamicsWorld::CreateDynamicsRobot( VirtualRobot::
std::vector<DynamicsRobotPtr> DynamicsWorld::getRobots()
{
- return engine->getRobots();
+ return engine->getRobots();
}
} // namespace
diff --git a/SimDynamics/DynamicsWorld.h b/SimDynamics/DynamicsWorld.h
index 25b4e9c37ac924754fe5bd1201b47f8515e30a75..316d54639c6a0aeab79b6cea1fa1190e4c35d044 100644
--- a/SimDynamics/DynamicsWorld.h
+++ b/SimDynamics/DynamicsWorld.h
@@ -118,7 +118,6 @@ protected:
// The engine interface
DynamicsEnginePtr engine;
-
};
}
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsViewer.ui b/SimDynamics/examples/SimDynamicsViewer/simDynamicsViewer.ui
index 04518929c3b59fbc1a99fbadaa2a97d85ac152ff..e0cfbeb918207fc8ff618ae91d3d6e31b550a394 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsViewer.ui
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsViewer.ui
@@ -29,58 +29,32 @@
<widget class="QGroupBox" name="groupBox">
<property name="maximumSize">
<size>
- <width>200</width>
+ <width>300</width>
<height>16777215</height>
</size>
</property>
<property name="title">
<string/>
</property>
- <widget class="QPushButton" name="pushButtonReset">
+ <widget class="QPushButton" name="pushButtonLoad">
<property name="geometry">
<rect>
<x>20</x>
<y>20</y>
- <width>171</width>
+ <width>101</width>
<height>28</height>
</rect>
</property>
<property name="text">
- <string>Reset</string>
- </property>
- </widget>
- <widget class="QLabel" name="label_10">
- <property name="geometry">
- <rect>
- <x>10</x>
- <y>270</y>
- <width>81</width>
- <height>16</height>
- </rect>
- </property>
- <property name="text">
- <string>Visualization</string>
- </property>
- </widget>
- <widget class="QCheckBox" name="checkBoxColModel">
- <property name="geometry">
- <rect>
- <x>20</x>
- <y>290</y>
- <width>141</width>
- <height>17</height>
- </rect>
- </property>
- <property name="text">
- <string>Collision Model</string>
+ <string>Load Robot</string>
</property>
</widget>
<widget class="QGroupBox" name="groupBox_2">
<property name="geometry">
<rect>
<x>10</x>
- <y>399</y>
- <width>181</width>
+ <y>300</y>
+ <width>281</width>
<height>181</height>
</rect>
</property>
@@ -90,9 +64,9 @@
<widget class="QLabel" name="label_RNTarget">
<property name="geometry">
<rect>
- <x>20</x>
+ <x>10</x>
<y>60</y>
- <width>151</width>
+ <width>161</width>
<height>16</height>
</rect>
</property>
@@ -103,9 +77,9 @@
<widget class="QLabel" name="label_RNVelocity">
<property name="geometry">
<rect>
- <x>20</x>
+ <x>10</x>
<y>80</y>
- <width>151</width>
+ <width>161</width>
<height>16</height>
</rect>
</property>
@@ -116,9 +90,9 @@
<widget class="QLabel" name="label_RNValue">
<property name="geometry">
<rect>
- <x>20</x>
+ <x>10</x>
<y>40</y>
- <width>151</width>
+ <width>161</width>
<height>16</height>
</rect>
</property>
@@ -129,9 +103,9 @@
<widget class="QLabel" name="label_RNName">
<property name="geometry">
<rect>
- <x>20</x>
+ <x>10</x>
<y>20</y>
- <width>151</width>
+ <width>161</width>
<height>16</height>
</rect>
</property>
@@ -139,13 +113,52 @@
<string>Name: <not set></string>
</property>
</widget>
+ <widget class="QLabel" name="label_RNPosVisu">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>120</y>
+ <width>261</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>VISU (simox):</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_RNPosCom">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>140</y>
+ <width>261</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>COM (bullet):</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_RNPosGP">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>100</y>
+ <width>261</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>GlobalPose (simox):</string>
+ </property>
+ </widget>
</widget>
<widget class="QComboBox" name="comboBoxRobotNode">
<property name="geometry">
<rect>
<x>20</x>
<y>80</y>
- <width>171</width>
+ <width>271</width>
<height>22</height>
</rect>
</property>
@@ -163,95 +176,179 @@
<string>RobotNode</string>
</property>
</widget>
- <widget class="QSlider" name="horizontalSliderTarget">
+ <widget class="QGroupBox" name="groupBox_3">
<property name="geometry">
<rect>
- <x>20</x>
- <y>150</y>
- <width>160</width>
- <height>19</height>
+ <x>10</x>
+ <y>490</y>
+ <width>281</width>
+ <height>91</height>
</rect>
</property>
- <property name="minimum">
- <number>-100</number>
- </property>
- <property name="maximum">
- <number>100</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
+ <property name="title">
+ <string>Visualization</string>
</property>
+ <widget class="QCheckBox" name="checkBoxCom">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>60</y>
+ <width>141</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>CoM</string>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxContacts">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>40</y>
+ <width>141</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Contacts</string>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxColModel">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>20</y>
+ <width>141</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Collision Model</string>
+ </property>
+ </widget>
</widget>
- <widget class="QLabel" name="label_8">
+ <widget class="QGroupBox" name="groupBox_4">
<property name="geometry">
<rect>
<x>20</x>
<y>120</y>
- <width>131</width>
- <height>16</height>
- </rect>
- </property>
- <property name="text">
- <string>Target Joint Value</string>
- </property>
- </widget>
- <widget class="QLabel" name="label_TargetMin">
- <property name="geometry">
- <rect>
- <x>20</x>
- <y>170</y>
- <width>41</width>
- <height>16</height>
- </rect>
- </property>
- <property name="text">
- <string>-1</string>
- </property>
- </widget>
- <widget class="QLabel" name="label_TargetMax">
- <property name="geometry">
- <rect>
- <x>150</x>
- <y>170</y>
- <width>31</width>
- <height>20</height>
- </rect>
- </property>
- <property name="layoutDirection">
- <enum>Qt::RightToLeft</enum>
- </property>
- <property name="text">
- <string>1</string>
- </property>
- </widget>
- <widget class="QCheckBox" name="checkBoxContacts">
- <property name="geometry">
- <rect>
- <x>20</x>
- <y>310</y>
- <width>141</width>
- <height>17</height>
- </rect>
- </property>
- <property name="text">
- <string>Contacts</string>
- </property>
- </widget>
- <widget class="QCheckBox" name="checkBoxActuation">
- <property name="geometry">
- <rect>
- <x>20</x>
- <y>220</y>
- <width>141</width>
- <height>17</height>
+ <width>271</width>
+ <height>171</height>
</rect>
</property>
- <property name="text">
- <string>Joint actuation</string>
- </property>
- <property name="checked">
- <bool>true</bool>
+ <property name="title">
+ <string>Physics</string>
</property>
+ <widget class="QCheckBox" name="checkBoxActuation">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>80</y>
+ <width>141</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Joint actuation</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_8">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>20</y>
+ <width>131</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Target Joint Value</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_TargetMin">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>60</y>
+ <width>81</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>-1</string>
+ </property>
+ </widget>
+ <widget class="QSlider" name="horizontalSliderTarget">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>40</y>
+ <width>241</width>
+ <height>19</height>
+ </rect>
+ </property>
+ <property name="minimum">
+ <number>-100</number>
+ </property>
+ <property name="maximum">
+ <number>100</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_TargetMax">
+ <property name="geometry">
+ <rect>
+ <x>190</x>
+ <y>60</y>
+ <width>71</width>
+ <height>20</height>
+ </rect>
+ </property>
+ <property name="layoutDirection">
+ <enum>Qt::RightToLeft</enum>
+ </property>
+ <property name="text">
+ <string>1</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ <widget class="QPushButton" name="pushButtonStartStop">
+ <property name="geometry">
+ <rect>
+ <x>24</x>
+ <y>120</y>
+ <width>91</width>
+ <height>31</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Stop Engine</string>
+ </property>
+ </widget>
+ <widget class="QPushButton" name="pushButtonStep">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="geometry">
+ <rect>
+ <x>140</x>
+ <y>120</y>
+ <width>91</width>
+ <height>31</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Step Engine</string>
+ </property>
+ </widget>
</widget>
</widget>
</item>
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
index e2ce1f3dae2ba8717beedd7a5504b72dce70f535..831c4c2ec29b84c250d01bcb409569c4da5d5bed 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
@@ -32,9 +32,12 @@ SimDynamicsWindow::SimDynamicsWindow(std::string &sRobotFilename, Qt::WFlags fla
useColModel = false;
VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(sRobotFilename);
- robotFilename = sRobotFilename;
- sceneSep = new SoSeparator;
- sceneSep->ref();
+ std::string robotFilename = sRobotFilename;
+ sceneSep = new SoSeparator;
+ sceneSep->ref();
+
+ comSep = new SoSeparator;
+ sceneSep->addChild(comSep);
contactsSep = new SoSeparator;
sceneSep->addChild(contactsSep);
@@ -56,10 +59,10 @@ SimDynamicsWindow::SimDynamicsWindow(std::string &sRobotFilename, Qt::WFlags fla
dynamicsWorld->addObject(dynamicsObject);
setupUI();
- loadRobot();
+ loadRobot(robotFilename);
// build visualization
- collisionModel();
+ buildVisualization();
viewer->viewAll();
@@ -91,6 +94,7 @@ void SimDynamicsWindow::timerCB(void * data, SoSensor * sensor)
window->updateJointInfo();
window->updateContactVisu();
+ window->updateComVisu();
}
@@ -101,9 +105,12 @@ void SimDynamicsWindow::setupUI()
viewer.reset(new SimDynamics::BulletCoinQtViewer(dynamicsWorld));
viewer->initSceneGraph(UI.frameViewer,sceneSep);
- connect(UI.checkBoxColModel, SIGNAL(clicked()), this, SLOT(collisionModel()));
- connect(UI.checkBoxActuation, SIGNAL(clicked()), this, SLOT(actuation()));
- connect(UI.pushButtonReset, SIGNAL(clicked()), this, SLOT(resetSceneryAll()));
+ connect(UI.checkBoxColModel, SIGNAL(clicked()), this, SLOT(buildVisualization()));
+ connect(UI.checkBoxActuation, SIGNAL(clicked()), this, SLOT(actuation()));
+ connect(UI.checkBoxCom, SIGNAL(clicked()), this, SLOT(comVisu()));
+ connect(UI.pushButtonLoad, SIGNAL(clicked()), this, SLOT(loadButton()));
+ connect(UI.pushButtonStartStop, SIGNAL(clicked()), this, SLOT(startStopEngine()));
+ connect(UI.pushButtonStep, SIGNAL(clicked()), this, SLOT(stepEngine()));
connect(UI.comboBoxRobotNode, SIGNAL(activated(int)), this, SLOT(selectRobotNode(int)));
connect(UI.horizontalSliderTarget, SIGNAL(valueChanged(int)), this, SLOT(jointValueChanged(int)));
@@ -154,7 +161,7 @@ void SimDynamicsWindow::actuation()
dynamicsRobot->disableActuation();
}
-void SimDynamicsWindow::collisionModel()
+void SimDynamicsWindow::buildVisualization()
{
if (!robot)
return;
@@ -166,6 +173,28 @@ void SimDynamicsWindow::collisionModel()
}
+void SimDynamicsWindow::comVisu()
+{
+ if (!robot)
+ return;
+ comSep->removeAllChildren();
+ comVisuMap.clear();
+ bool visuCom = UI.checkBoxCom->checkState() == Qt::Checked;
+ if (visuCom)
+ {
+ std::vector<RobotNodePtr> n = robot->getRobotNodes();
+ for (size_t i=0;i<n.size();i++)
+ {
+ SoSeparator* sep = new SoSeparator;
+ comSep->addChild(sep);
+ Eigen::Matrix4f cp = dynamicsRobot->getComGlobal(n[i]);
+ sep->addChild(CoinVisualizationFactory::getMatrixTransformM(cp));
+ sep->addChild(CoinVisualizationFactory::CreateCoordSystemVisualization(5.0f));
+ comVisuMap[n[i]] = sep;
+ }
+ }
+}
+
void SimDynamicsWindow::closeEvent(QCloseEvent *event)
{
quit();
@@ -227,7 +256,7 @@ void SimDynamicsWindow::updateJoints()
selectRobotNode(-1);
}
-void SimDynamicsWindow::loadRobot()
+bool SimDynamicsWindow::loadRobot(std::string robotFilename)
{
cout << "Loading Robot from " << robotFilename << endl;
@@ -239,23 +268,23 @@ void SimDynamicsWindow::loadRobot()
{
cout << " ERROR while creating robot" << endl;
cout << e.what();
- return;
+ return false;
}
if (!robot)
{
cout << " ERROR while creating robot" << endl;
- return;
+ return false;
}
//robot->print();
Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
- gp(2,3) = 20.0f;
+ gp(2,3) = 200.0f;
robot->setGlobalPose(gp);
dynamicsRobot = dynamicsWorld->CreateDynamicsRobot(robot);
dynamicsWorld->addRobot(dynamicsRobot);
updateJoints();
-
+ return true;
}
void SimDynamicsWindow::selectRobotNode( int n )
@@ -269,14 +298,11 @@ void SimDynamicsWindow::selectRobotNode( int n )
if (rn)
{
int pos = 0;
- if (rn->getJointValue()<0 && rn->getJointLimitLo()<0)
- {
- pos = int(rn->getJointValue()/rn->getJointLimitLo() * 100.0f + 0.5f);
- }
- if (rn->getJointValue()>0 && rn->getJointLimitHi()>0)
- {
- pos = int(rn->getJointValue()/rn->getJointLimitHi() * 100.0f + 0.5f);
- }
+
+ float l = rn->getJointLimitHi()-rn->getJointLimitLo();
+ float start = rn->getJointValue() - rn->getJointLimitLo();
+ if (fabs(l)>1e-6)
+ pos = int(start/l * 200.0f +0.5f) - 100;
UI.horizontalSliderTarget->setValue(pos);
UI.horizontalSliderTarget->setEnabled(true);
} else
@@ -288,14 +314,18 @@ void SimDynamicsWindow::selectRobotNode( int n )
void SimDynamicsWindow::updateJointInfo()
{
+ std::stringstream info;
int n = UI.comboBoxRobotNode->currentIndex();
QString qMin("0");
QString qMax("0");
QString qName("Name: <not set>");
QString qJV("Joint value: 0");
QString qTarget("Joint target: 0");
- QString qVel("Joint velocity: 0");
-
+ QString qVel("Joint velocity: 0");
+ QString qGP("GlobalPose (simox): 0/0/0");
+ QString qVisu("VISU (simox): 0/0/0");
+ QString qCom("COM (bullet): 0/0/0");
+ QString tmp;
RobotNodeRevolutePtr rn;
if (n>=0 && n<(int)robotNodes.size())
{
@@ -304,19 +334,61 @@ void SimDynamicsWindow::updateJointInfo()
//SimDynamics::DynamicsObjectPtr dynRN = dynamicsRobot->getDynamicsRobotNode(rn);
if (rn)
{
- qMin = QString::number(rn->getJointLimitLo(),'g',4);
- qMax = QString::number(rn->getJointLimitHi(),'g',4);
+ qMin = QString::number(rn->getJointLimitLo(),'f',4);
+ qMax = QString::number(rn->getJointLimitHi(),'f',4);
qName = QString("Name: ");
qName += QString(rn->getName().c_str());
qJV = QString("Joint value: ");
- qJV += QString::number(rn->getJointValue(),'g',3);
+ tmp = QString::number(rn->getJointValue(),'f',3);
+ qJV += tmp;
+ info << "jv rn:" << tmp.toStdString();
+
qJV += QString(" / ");
- qJV += QString::number(dynamicsRobot->getJointAngle(rn),'g',3);
+ tmp = QString::number(dynamicsRobot->getJointAngle(rn),'f',3);
+ qJV += tmp;
+ info << ",\tjv bul:" << tmp.toStdString();
+
qTarget = QString("Joint target: ");
- qTarget += QString::number(dynamicsRobot->getNodeTarget(rn),'g',4);
+ tmp = QString::number(dynamicsRobot->getNodeTarget(rn),'f',3);
+ qTarget +=tmp;
+ info << ",targ:" << tmp.toStdString();
+
qVel = QString("Joint velocity: ");
- qVel += QString::number(dynamicsRobot->getJointSpeed(rn),'g',4);
-
+ tmp = QString::number(dynamicsRobot->getJointSpeed(rn),'f',3);
+ qVel +=tmp;
+ info << ",vel:" << tmp.toStdString();
+ Eigen::Matrix4f gp = rn->getGlobalPose();
+
+ qGP = QString("GlobalPose (simox):");
+ tmp = QString::number(gp(0,3),'f',2);
+ qGP += tmp;
+ info << ",gp:" << tmp.toStdString();
+
+ qGP += QString("/");
+ tmp = QString::number(gp(1,3),'f',2);
+ qGP += tmp;
+ info << "/" << tmp.toStdString();
+
+ qGP += QString("/");
+ tmp = QString::number(gp(2,3),'f',2);
+ qGP += tmp;
+ info << "/" << tmp.toStdString();
+
+ gp = rn->getGlobalPoseVisualization();
+ qVisu = QString("VISU (simox):");
+ qVisu += QString::number(gp(0,3),'f',2);
+ qVisu += QString("/");
+ qVisu += QString::number(gp(1,3),'f',2);
+ qVisu += QString("/");
+ qVisu += QString::number(gp(2,3),'f',2);
+
+ gp = dynamicsRobot->getComGlobal(rn);
+ qCom = QString("COM (bullet):");
+ qCom += QString::number(gp(0,3),'f',2);
+ qCom += QString("/");
+ qCom += QString::number(gp(1,3),'f',2);
+ qCom += QString("/");
+ qCom += QString::number(gp(2,3),'f',2);
}
UI.label_TargetMin->setText(qMin);
UI.label_TargetMax->setText(qMax);
@@ -324,6 +396,13 @@ void SimDynamicsWindow::updateJointInfo()
UI.label_RNValue->setText(qJV);
UI.label_RNTarget->setText(qTarget);
UI.label_RNVelocity->setText(qVel);
+ UI.label_RNPosGP->setText(qGP);
+ UI.label_RNPosVisu->setText(qVisu);
+ UI.label_RNPosCom->setText(qCom);
+
+ // print some joint info
+ if (viewer->engineRunning())
+ cout << info.str() << endl;
}
void SimDynamicsWindow::jointValueChanged( int n )
@@ -338,14 +417,9 @@ void SimDynamicsWindow::jointValueChanged( int n )
return;
float pos = 0;
- if (n<0)
- {
- pos = (float(n) / 100.0f * fabs(rn->getJointLimitLo()));
- }
- if (n>0)
- {
- pos = (float(n) / 100.0f * fabs(rn->getJointLimitHi()));
- }
+
+ float l = rn->getJointLimitHi()-rn->getJointLimitLo();
+ pos = float(n+100)/201.0f * l + rn->getJointLimitLo();
dynamicsRobot->actuateNode(rn,pos);
}
@@ -397,3 +471,60 @@ void SimDynamicsWindow::updateContactVisu()
}
}
+void SimDynamicsWindow::updateComVisu()
+{
+ std::vector<RobotNodePtr> n = robot->getRobotNodes();
+ std::map< VirtualRobot::RobotNodePtr, SoSeparator* >::iterator i = comVisuMap.begin();
+ while (i!=comVisuMap.end())
+ {
+ SoSeparator* sep = i->second;
+ SoMatrixTransform *m = dynamic_cast<SoMatrixTransform*>(sep->getChild(0));
+ if (m)
+ {
+ Eigen::Matrix4f ma = dynamicsRobot->getComGlobal(i->first);
+ ma.block(0,3,3,1) *= 0.001f;
+ m->matrix.setValue(CoinVisualizationFactory::getSbMatrix(ma));
+ }
+ i++;
+ }
+}
+
+void SimDynamicsWindow::loadButton()
+{
+ QString fileName = QFileDialog::getOpenFileName(this,
+ tr("Select Robot File"), "",
+ tr("Simox Robot File (*.xml)"));
+ std::string f = fileName.toStdString();
+ if (RuntimeEnvironment::getDataFileAbsolute(f))
+ {
+ if (dynamicsRobot)
+ {
+ viewer->removeVisualization(dynamicsRobot);
+ dynamicsWorld->removeRobot(dynamicsRobot);
+ }
+ dynamicsRobot.reset();
+ loadRobot(f);
+ buildVisualization();
+ }
+}
+
+void SimDynamicsWindow::startStopEngine()
+{
+ if (viewer->engineRunning())
+ {
+ UI.pushButtonStartStop->setText(QString("Start Engine"));
+ UI.pushButtonStep->setEnabled(true);
+ viewer->stopEngine();
+ } else
+ {
+ UI.pushButtonStartStop->setText(QString("Stop Engine"));
+ UI.pushButtonStep->setEnabled(false);
+ viewer->startEngine();
+ }
+}
+
+void SimDynamicsWindow::stepEngine()
+{
+ viewer->stepPhysics();
+}
+
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
index 34d51a1bffd7d716faa1febc6e10ce29e90eee95..cfd64cf0c77f7da893480b3a69954771b140c65d 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
@@ -49,22 +49,27 @@ public slots:
void closeEvent(QCloseEvent *event);
void resetSceneryAll();
- void collisionModel();
+ void buildVisualization();
void actuation();
- void loadRobot();
+ void loadButton();
void selectRobotNode(int n);
void jointValueChanged(int n);
-
+ void comVisu();
void updateJointInfo();
+
+ void startStopEngine();
+ void stepEngine();
protected:
+ bool loadRobot(std::string robotFilename);
void setupUI();
void updateJoints();
void stopCB();
void updateContactVisu();
+ void updateComVisu();
SimDynamics::DynamicsWorldPtr dynamicsWorld;
SimDynamics::DynamicsRobotPtr dynamicsRobot;
@@ -72,20 +77,23 @@ protected:
Ui::MainWindowBulletViewer UI;
- SoSeparator* sceneSep;
+ SoSeparator* sceneSep;
+ SoSeparator* comSep;
SoSeparator* contactsSep;
SimDynamics::BulletCoinQtViewerPtr viewer;
VirtualRobot::RobotPtr robot;
- std::string robotFilename;
// beside the viewer cb we need also a callback to update joint info
static void timerCB(void * data, SoSensor * sensor);
- SoTimerSensor *timerSensor;
+
+ SoTimerSensor *timerSensor;
std::vector<VirtualRobot::RobotNodeRevolutePtr> robotNodes;
+ std::map< VirtualRobot::RobotNodePtr, SoSeparator* > comVisuMap;
+
bool useColModel;
};
diff --git a/VirtualRobot/Nodes/RobotNodeActuator.cpp b/VirtualRobot/Nodes/RobotNodeActuator.cpp
index 9b993017626b97b4c3653adb2182bc3cd7d97820..cbb1d701131f9137206ea30c609d2767c4e6477d 100644
--- a/VirtualRobot/Nodes/RobotNodeActuator.cpp
+++ b/VirtualRobot/Nodes/RobotNodeActuator.cpp
@@ -23,5 +23,10 @@ void RobotNodeActuator::updateVisualizationPose( const Eigen::Matrix4f& pose, fl
robotNode->updateVisualizationPose(pose, jointValue, updateChildren);
}
+VirtualRobot::RobotNodePtr RobotNodeActuator::getRobotNode()
+{
+ return robotNode;
+}
+
} // namespace
diff --git a/VirtualRobot/Nodes/RobotNodeActuator.h b/VirtualRobot/Nodes/RobotNodeActuator.h
index 46dc0d245f474a65e91452b32f334dd731f2e103..5032e4eb39f35238a41243302fc33495f1f7bde6 100644
--- a/VirtualRobot/Nodes/RobotNodeActuator.h
+++ b/VirtualRobot/Nodes/RobotNodeActuator.h
@@ -48,6 +48,7 @@ public:
virtual void updateVisualizationPose (const Eigen::Matrix4f& pose, bool updateChildren = false);
virtual void updateVisualizationPose (const Eigen::Matrix4f& pose, float jointValue, bool updateChildren = false);
+ RobotNodePtr getRobotNode();
protected:
RobotNodePtr robotNode;
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h
index c917291ff115d79f1913e017ebef250abe5b30a3..f5e572ad2d4e906bb8f86c4ee877ed6cebc84f4a 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h
@@ -32,6 +32,7 @@ class SoNode;
namespace VirtualRobot
{
+
/*!
A Coin3D based implementation of a visualization.
*/
diff --git a/VirtualRobot/Visualization/Visualization.h b/VirtualRobot/Visualization/Visualization.h
index af5af441834530c530421c4e0ddef1291db8ff0a..9547fea4942fb9e693915b22b66860ba147c897a 100644
--- a/VirtualRobot/Visualization/Visualization.h
+++ b/VirtualRobot/Visualization/Visualization.h
@@ -47,7 +47,7 @@ public:
virtual bool highlight(VisualizationNodePtr visualizationNode, bool enable);
/*!
- Highlight a visualization node.
+ Highlight a visualization node.
@param which The index of the visualionNodes, passed to the constructor.
@param enable Do/Undo highlighting.
diff --git a/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml b/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml
index 7a889fbd31a8497b8682fc7a628a3958b1f5a74b..95cd052bc30882c799ffa891bf9c331b84b35dfb 100644
--- a/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml
+++ b/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml
@@ -138,9 +138,9 @@
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
<File type="Inventor">fullmodel/wrist2_l.iv</File>
</Visualization>
- <CollisionModel>
+ <!--CollisionModel>
<File type="Inventor">collisionModel/wrist2_l.iv</File>
- </CollisionModel>
+ </CollisionModel-->
<ChildFromRobot>
<File importEEF="true">ArmarIII-LeftHand.xml</File>
diff --git a/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml b/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml
index 8cbf7ceafdcf0483d7f7747fc02a80b77d38b0a4..31f9ef21a050c35fd68d58988d76effe00af3008 100644
--- a/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml
+++ b/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml
@@ -126,9 +126,9 @@
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
<File type="Inventor">fullmodel/wrist2_r.iv</File>
</Visualization>
- <CollisionModel>
+ <!--CollisionModel>
<File type="Inventor">collisionModel/wrist2_l.iv</File>
- </CollisionModel>
+ </CollisionModel-->
<Physics>
<CoM location="VisualizationBBoxCenter"/>
<Mass value="2.59945309" units="kg" />
diff --git a/VirtualRobot/data/robots/examples/SimpleRobot/Joint3DH.xml b/VirtualRobot/data/robots/examples/SimpleRobot/Joint3DH.xml
index b3e879fab8ca67f750befd1ebe454783d73073fe..7f71d57ca8439c6bfd94f4326a14602ad7c926e4 100644
--- a/VirtualRobot/data/robots/examples/SimpleRobot/Joint3DH.xml
+++ b/VirtualRobot/data/robots/examples/SimpleRobot/Joint3DH.xml
@@ -16,7 +16,7 @@
<RobotNode name="Joint1">
<Joint type="revolute">
<Limits unit="degree" lo="-90" hi="90"/>
- <DH a="300" d="0" theta="0" alpha="90" units="degree"/>
+ <DH a="300" d="0" theta="0" alpha="45" units="degree"/>
</Joint>
<Visualization enable="true">
<File type="inventor">joint_rot_sphere.iv</File>
@@ -32,7 +32,7 @@
<RobotNode name="Joint2">
<Joint type="revolute">
<Limits unit="degree" lo="-180" hi="45"/>
- <DH a="300" d="0" theta="90" alpha="0" units="degree"/>
+ <DH a="300" d="0" theta="50" alpha="0" units="degree"/>
</Joint>
<Visualization enable="true">
<File type="inventor">joint_rot_sphere.iv</File>