From 4454a32ee6afdf4aec03e448a645ea75100461df Mon Sep 17 00:00:00 2001
From: vahrenkamp <vahrenkamp@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Wed, 13 Aug 2014 09:21:20 +0000
Subject: [PATCH] Added several nan checks. Added mutex protection for
SimDynamics objects.
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@669 042f3d55-54a8-47e9-b7fb-15903f145c44
---
CMakeLists.txt | 1 +
SimDynamics/CMakeLists.txt | 3 +-
.../BulletEngine/BulletEngine.cpp | 39 +++---
.../BulletEngine/BulletEngine.h | 7 +-
.../BulletEngine/BulletObject.cpp | 10 ++
.../BulletEngine/BulletRobot.cpp | 122 ++++++++++++++++--
.../BulletEngine/BulletRobotLogger.h | 39 ++++--
.../BulletEngine/SimoxMotionState.cpp | 13 ++
SimDynamics/DynamicsEngine/DynamicsEngine.cpp | 55 +++++---
SimDynamics/DynamicsEngine/DynamicsEngine.h | 28 +++-
SimDynamics/DynamicsEngine/DynamicsObject.cpp | 10 ++
SimDynamics/DynamicsEngine/DynamicsObject.h | 7 +
SimDynamics/DynamicsEngine/DynamicsRobot.cpp | 28 +++-
SimDynamics/DynamicsEngine/DynamicsRobot.h | 5 +
SimDynamics/DynamicsEngine/DynamicsUtils.cpp | 15 ++-
SimDynamics/DynamicsEngine/DynamicsUtils.h | 17 ++-
VirtualRobot/IK/DifferentialIK.cpp | 10 +-
17 files changed, 331 insertions(+), 78 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ea6982ca7..a136b7923 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -42,6 +42,7 @@ if (Simox_BUILD_SimDynamics)
list (APPEND Simox_EXTERNAL_LIBRARIES ${Simox_SimDynamics_EXTERNAL_LIBRARIES})
list (APPEND Simox_EXTERNAL_INCLUDE_DIRS ${Simox_SimDynamics_INCLUDE_DIRS})
+ list (APPEND Simox_EXTERNAL_LIBRARY_FLAGS ${Simox_SimDynamics_COMPILE_FLAGS})
diff --git a/SimDynamics/CMakeLists.txt b/SimDynamics/CMakeLists.txt
index 25f228a21..c0e36eddc 100644
--- a/SimDynamics/CMakeLists.txt
+++ b/SimDynamics/CMakeLists.txt
@@ -44,7 +44,7 @@ if (SimDynamics_USE_BULLET)
endif()
IF( SimDynamics_USE_BULLET_DOUBLE_PRECISION )
ADD_DEFINITIONS( -DBT_USE_DOUBLE_PRECISION)
- SET(SIMDYNAMICS_COMPILE_FLAGS "${SIMDYNAMICS_COMPILE_FLAGS}" "-DBT_USE_DOUBLE_PRECISION" PARENT_SCOPE)
+ SET(SIMDYNAMICS_COMPILE_FLAGS "${SIMDYNAMICS_COMPILE_FLAGS}" "-DBT_USE_DOUBLE_PRECISION")
ENDIF( SimDynamics_USE_BULLET_DOUBLE_PRECISION )
#MESSAGE(STATUS "BULLET_LIBRARIES:${BULLET_LIBRARIES}")
SET (SimDynamics_PHYSICS_LIBRARIES "${BULLET_LIBRARIES}")
@@ -203,6 +203,7 @@ if (SimDynamics_DYNAMICSENGINE)
# this var is considered for generating Simox_INCLUDE_DIRS_
SET(Simox_SimDynamics_INCLUDE_DIRS "${SIMDYNAMICS_INCLUDE_DIRS}" PARENT_SCOPE)
SET(Simox_SimDynamics_EXTERNAL_LIBRARIES "${SimDynamics_PHYSICS_LIBRARIES}" PARENT_SCOPE)
+ SET(Simox_SimDynamics_COMPILE_FLAGS "${SIMDYNAMICS_COMPILE_FLAGS}" PARENT_SCOPE)
#INCLUDE_DIRECTORIES(${SimDynamics_SimoxDir})
#INCLUDE_DIRECTORIES(${SimDynamics_DIR})
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
index fa4bdde87..e5cecca95 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
@@ -9,7 +9,8 @@
namespace SimDynamics {
-BulletEngine::BulletEngine()
+BulletEngine::BulletEngine(boost::shared_ptr <boost::recursive_mutex> engineMutex)
+ :DynamicsEngine(engineMutex)
{
collision_config = NULL;
dispatcher = NULL;
@@ -42,7 +43,7 @@ bool BulletEngine::init( DynamicsEngineConfigPtr config )
bool BulletEngine::init( BulletEngineConfigPtr config )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
DynamicsEngine::init(config);
bulletConfig = config;
@@ -93,7 +94,7 @@ bool BulletEngine::init( BulletEngineConfigPtr config )
bool BulletEngine::cleanup()
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
while (robots.size()>0)
{
size_t start = robots.size();
@@ -125,7 +126,7 @@ bool BulletEngine::cleanup()
void BulletEngine::updateConfig(BulletEngineConfigPtr newConfig)
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
bulletConfig = newConfig;
@@ -157,7 +158,7 @@ void BulletEngine::updateConfig(BulletEngineConfigPtr newConfig)
bool BulletEngine::addObject( DynamicsObjectPtr o )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
BulletObjectPtr btObject = boost::dynamic_pointer_cast<BulletObject>(o);
if (!btObject)
{
@@ -202,7 +203,7 @@ bool BulletEngine::addObject( DynamicsObjectPtr o )
bool BulletEngine::removeObject( DynamicsObjectPtr o )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
BulletObjectPtr btObject = boost::dynamic_pointer_cast<BulletObject>(o);
if (!btObject)
{
@@ -216,7 +217,7 @@ bool BulletEngine::removeObject( DynamicsObjectPtr o )
bool BulletEngine::removeLink( BulletRobot::LinkInfo &l )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
dynamicsWorld->removeConstraint(l.joint.get());
this->resetCollisions(static_cast<DynamicsObject*>(l.dynNode1.get()));
this->resetCollisions(static_cast<DynamicsObject*>(l.dynNode2.get()));
@@ -230,7 +231,7 @@ btDynamicsWorld* BulletEngine::getBulletWorld()
void BulletEngine::createFloorPlane( const Eigen::Vector3f &pos, const Eigen::Vector3f &up )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
DynamicsEngine::createFloorPlane(pos,up);
float size = float(floorExtendMM);//50000.0f; // mm
float sizeSmall = float(floorDepthMM);// 500.0f;
@@ -348,7 +349,7 @@ Eigen::Vector3f BulletEngine::getVecEigen( const btVector3 &vec, bool scaling )
bool BulletEngine::addRobot( DynamicsRobotPtr r )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
BulletRobotPtr btRobot = boost::dynamic_pointer_cast<BulletRobot>(r);
if (!btRobot)
{
@@ -373,7 +374,7 @@ bool BulletEngine::addRobot( DynamicsRobotPtr r )
void BulletEngine::addExternalCallback(BulletStepCallback function, void* data)
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
callbacks.push_back(ExCallbackData(function, data));
}
@@ -381,7 +382,11 @@ void BulletEngine::addExternalCallback(BulletStepCallback function, void* data)
void BulletEngine::externalCallbacks(btDynamicsWorld *world, btScalar timeStep)
{
BulletEngine *e = static_cast<BulletEngine *>(world->getWorldUserInfo());
- boost::recursive_mutex::scoped_lock scoped_lock(e->engineMutex);
+ if (!e)
+ return;
+
+ // apply lock
+ MutexLockPtr lock = e->getScopedLock();
e->updateRobots(timeStep);
@@ -403,7 +408,7 @@ void BulletEngine::updateRobots(btScalar timeStep)
bool BulletEngine::removeRobot( DynamicsRobotPtr r )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
BulletRobotPtr btRobot = boost::dynamic_pointer_cast<BulletRobot>(r);
if (!btRobot)
{
@@ -428,7 +433,7 @@ bool BulletEngine::removeRobot( DynamicsRobotPtr r )
bool BulletEngine::addLink( BulletRobot::LinkInfo &l )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
#ifdef DEBUG_FIXED_OBJECTS
cout << "TEST2" << endl;
#else
@@ -443,7 +448,7 @@ bool BulletEngine::addLink( BulletRobot::LinkInfo &l )
void BulletEngine::print()
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
cout << "------------------ Bullet Engine ------------------" << endl;
for (size_t i=0;i<objects.size();i++)
{
@@ -499,7 +504,7 @@ void BulletEngine::print()
void BulletEngine::activateAllObjects()
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
for (size_t i=0;i<objects.size();i++)
{
BulletObjectPtr bo = boost::dynamic_pointer_cast<BulletObject>(objects[i]);
@@ -513,7 +518,7 @@ void BulletEngine::activateAllObjects()
std::vector<DynamicsEngine::DynamicsContactInfo> BulletEngine::getContacts()
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
//Assume world->stepSimulation or world->performDiscreteCollisionDetection has been called
std::vector<DynamicsEngine::DynamicsContactInfo> result;
@@ -559,7 +564,7 @@ std::vector<DynamicsEngine::DynamicsContactInfo> BulletEngine::getContacts()
void BulletEngine::stepSimulation( double dt, int maxSubSteps, double fixedTimeStep )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
simTime += dt;
dynamicsWorld->stepSimulation(btScalar(dt), maxSubSteps, btScalar(fixedTimeStep));
}
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
index 772c7b20e..b143f5d68 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
@@ -89,9 +89,10 @@ public:
friend class BulletObject;
/*!
- Constructor
- */
- BulletEngine();
+ Constructor
+ \param engineMutex Optionally, all engine access methods can be protected by an external mutex. If not set, an internal mutex is creeated.
+ */
+ BulletEngine(boost::shared_ptr <boost::recursive_mutex> engineMutex = boost::shared_ptr<boost::recursive_mutex>());
/*!
*/
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
index ee682a7f1..2d65b6d05 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
@@ -223,6 +223,7 @@ boost::shared_ptr<btRigidBody> BulletObject::getRigidBody()
void BulletObject::setPosition( const Eigen::Vector3f &posMM )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
pose.block(0,3,3,1) = posMM;
setPose(pose);
@@ -230,6 +231,7 @@ void BulletObject::setPosition( const Eigen::Vector3f &posMM )
void BulletObject::setPoseIntern( const Eigen::Matrix4f &pose )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
/* convert to local coord system, apply comoffset and convert back*/
Eigen::Matrix4f poseLocal = sceneObject->getGlobalPose().inverse() * pose;
poseLocal.block(0, 3, 3, 1) += com;
@@ -242,12 +244,14 @@ void BulletObject::setPoseIntern( const Eigen::Matrix4f &pose )
void BulletObject::setPose( const Eigen::Matrix4f &pose )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
DynamicsObject::setPose(pose);
setPoseIntern(pose);
}
Eigen::Vector3f BulletObject::getLinearVelocity()
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
if (!rigidBody)
return Eigen::Vector3f::Zero();
return (BulletEngine::getVecEigen(rigidBody->getLinearVelocity()));
@@ -255,6 +259,7 @@ Eigen::Vector3f BulletObject::getLinearVelocity()
Eigen::Vector3f BulletObject::getAngularVelocity()
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
if (!rigidBody)
return Eigen::Vector3f::Zero();
return (BulletEngine::getVecEigen(rigidBody->getAngularVelocity(),false));
@@ -262,6 +267,7 @@ Eigen::Vector3f BulletObject::getAngularVelocity()
void BulletObject::setLinearVelocity( const Eigen::Vector3f &vel )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
if (!rigidBody)
return;
btVector3 btVel = BulletEngine::getVecBullet(vel,false);
@@ -270,6 +276,7 @@ void BulletObject::setLinearVelocity( const Eigen::Vector3f &vel )
void BulletObject::setAngularVelocity( const Eigen::Vector3f &vel )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
if (!rigidBody)
return;
btVector3 btVel = BulletEngine::getVecBullet(vel,false);
@@ -278,6 +285,7 @@ void BulletObject::setAngularVelocity( const Eigen::Vector3f &vel )
Eigen::Matrix4f BulletObject::getComGlobal()
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
btTransform tr;
motionState->getWorldTransform(tr);
return BulletEngine::getPoseEigen(tr);
@@ -285,6 +293,7 @@ Eigen::Matrix4f BulletObject::getComGlobal()
void BulletObject::applyForce(const Eigen::Vector3f &force)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
if (!rigidBody)
return;
btVector3 btVel = BulletEngine::getVecBullet(force,false);
@@ -293,6 +302,7 @@ void BulletObject::applyForce(const Eigen::Vector3f &force)
void BulletObject::applyTorque(const Eigen::Vector3f &torque)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
if (!rigidBody)
return;
btVector3 btVel = BulletEngine::getVecBullet(torque,false);
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index 10e71148b..cfcbc661c 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -20,6 +20,8 @@
#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>
+#include <boost/math/special_functions/fpclassify.hpp>
+
//#define DEBUG_FIXED_OBJECTS
//#define DEBUG_SHOW_LINKS
using namespace VirtualRobot;
@@ -30,7 +32,7 @@ namespace SimDynamics {
BulletRobot::BulletRobot(VirtualRobot::RobotPtr rob, bool enableJointMotors)
: DynamicsRobot(rob)
{
- bulletMaxMotorImulse = 100.0f;
+ bulletMaxMotorImulse = 800.0f;
buildBulletModels(enableJointMotors);
@@ -49,6 +51,20 @@ BulletRobot::BulletRobot(VirtualRobot::RobotPtr rob, bool enableJointMotors)
std::cout << "Found force torque sensor: " << node->getName() << std::endl;
}
}
+#ifdef _DEBUG
+ std::string nameBodies = "BulletRobot_RNS_Bodies_All";
+ std::vector<RobotNodePtr> rnAll = robot->getRobotNodes();
+ std::vector<RobotNodePtr> rnsBodies;
+ for (size_t i=0;i<rnAll.size();i++)
+ {
+ RobotNodePtr r = rnAll[i];
+ //if (r->isRotationalJoint() || r->isTranslationalJoint())
+ // rnsJoints.push_back(r);
+ if (r->getMass()>0)
+ rnsBodies.push_back(r);
+ }
+ RobotNodeSetPtr rnsB = RobotNodeSet::createRobotNodeSet(getRobot(),nameBodies,rnsBodies,RobotNodePtr(),RobotNodePtr(),true);
+#endif
}
BulletRobot::~BulletRobot()
@@ -57,6 +73,7 @@ BulletRobot::~BulletRobot()
void BulletRobot::buildBulletModels(bool enableJointMotors)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
if (!robot)
return;
@@ -130,6 +147,7 @@ void BulletRobot::buildBulletModels(bool enableJointMotors)
void BulletRobot::addIgnoredCollisionModels(RobotNodePtr rn)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT (rn);
if (!rn->getCollisionModel())
return; // nothing to do: no col model -> no bullet model -> no collisions
@@ -157,6 +175,7 @@ void BulletRobot::addIgnoredCollisionModels(RobotNodePtr rn)
void BulletRobot::createLink( VirtualRobot::RobotNodePtr bodyA, VirtualRobot::RobotNodePtr joint, VirtualRobot::RobotNodePtr joint2, VirtualRobot::RobotNodePtr bodyB, bool enableJointMotors )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
// ensure dynamics nodes are created
createDynamicsNode(bodyA);
createDynamicsNode(bodyB);
@@ -359,6 +378,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr bodyA, VirtualRobot::Ro
bool BulletRobot::hasLink( VirtualRobot::RobotNodePtr node1, VirtualRobot::RobotNodePtr node2 )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
for (size_t i=0; i<links.size();i++)
{
if (links[i].nodeA == node1 && links[i].nodeB == node2)
@@ -392,15 +412,17 @@ void BulletRobot::ensureKinematicConstraints()
void BulletRobot::actuateJoints(double dt)
{
- //cout << "=== === BulletRobot: actuateJoints() 1 === " << endl;
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
+ //cout << "=== === BulletRobot: actuateJoints() 1 === " << this << endl;
std::map<VirtualRobot::RobotNodePtr, robotNodeActuationTarget>::iterator it = actuationTargets.begin();
- int jointCounter = 0;
+ //int jointCounter = 0;
//cout << "**** Control Values: ";
for (; it != actuationTargets.end(); it++)
{
+ //cout << "it:" << it->first << ", name: " << it->first->getName() << endl;
VelocityMotorController& controller = actuationControllers[it->first];
if (it->second.node->isRotationalJoint())
@@ -411,6 +433,7 @@ void BulletRobot::actuateJoints(double dt)
btScalar posTarget = btScalar(it->second.jointValueTarget + link.jointValueOffset);
btScalar posActual = btScalar(getJointAngle(it->first));
+ btScalar velActual = btScalar(getJointSpeed(it->first));
btScalar velocityTarget = btScalar(it->second.jointVelocityTarget);
#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
@@ -450,19 +473,24 @@ void BulletRobot::actuateJoints(double dt)
double targetVelocity;
if (actuation.modes.position && actuation.modes.velocity)
{
+ //cout << "################### posActual:" << posActual << ", posTarget" << posTarget << ", velTarget:" << velocityTarget << endl;
targetVelocity = controller.update(posTarget - posActual, velocityTarget, actuation, btScalar(dt));
}
else if (actuation.modes.position)
{
- targetVelocity = controller.update(posTarget - posActual, 0.0, actuation, btScalar(dt));
+ // cout << "################### posActual:" << posActual << ", posTarget" << posTarget << endl;
+ targetVelocity = controller.update(posTarget - posActual, 0.0, actuation, btScalar(dt));
}
else if (actuation.modes.velocity)
{
+ //cout << "################### velActual:" << velActual << ", velTarget" << velocityTarget << endl;
+ controller.setCurrentVelocity(velActual);
targetVelocity = controller.update(0.0, velocityTarget, actuation, btScalar(dt));
}
// FIXME this bypasses the controller (and doesn't work..)
else if (actuation.modes.torque)
{
+ //cout << "################### torque:" << it->second.jointTorqueTarget << endl;
targetVelocity = it->second.jointTorqueTarget;
//cout << "jointTorqueTarget for joint " << it->second.node->getName() << " :" << it->second.jointTorqueTarget << endl;
/*
@@ -504,7 +532,38 @@ void BulletRobot::actuateJoints(double dt)
jointCounter++;
*/
}
- hinge->enableAngularMotor(true, btScalar(targetVelocity), bulletMaxMotorImulse);
+
+ btScalar maxImpulse = bulletMaxMotorImulse;
+ /*if (it->second.node->getMaxTorque()>0)
+ {
+ maxImpulse = it->second.node->getMaxTorque() * btScalar(dt);
+ }*/
+#ifdef _DEBUG
+ if (it->first->getName() == "Elbow R")
+ cout << "################### " << it->first->getName() <<": posActual:" << posActual << ", posTarget:" << posTarget << ", actvel:" << velActual << ", target vel:" << targetVelocity << ", maxImpulse" << maxImpulse << endl;
+#endif
+#ifdef _DEBUG
+ std::string nameBodies = "BulletRobot_RNS_Bodies_All";
+ VirtualRobot::RobotNodeSetPtr rnsBodies = robot->getRobotNodeSet(nameBodies);
+ Eigen::Vector3f v = getComVelocityGlobal(rnsBodies);
+ if (boost::math::isnan(v(0)) || boost::math::isnan(v(1)) || boost::math::isnan(v(2)))
+ {
+ VR_ERROR << "NAN before bullet call!!!" << endl;
+ }
+#endif
+ hinge->enableAngularMotor(true, btScalar(targetVelocity), maxImpulse);
+#ifdef _DEBUG
+ //std::string nameBodies = "BulletRobot_RNS_Bodies_All";
+ //VirtualRobot::RobotNodeSetPtr rnsBodies = robot->getRobotNodeSet(nameBodies);
+ v = getComVelocityGlobal(rnsBodies);
+ if (boost::math::isnan(v(0)) || boost::math::isnan(v(1)) || boost::math::isnan(v(2)))
+ {
+ VR_ERROR << "NAN after bullet call!!!" << endl;
+ }
+#endif
+
+
+
#endif
}
}
@@ -514,6 +573,7 @@ void BulletRobot::actuateJoints(double dt)
void BulletRobot::updateSensors(double dt)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
boost::unordered_set<std::string> contactObjectNames;
// this seems stupid and it is, but that is abstract interfaces for you.
for(std::vector<SensorPtr>::iterator it = sensors.begin(); it != sensors.end(); it++)
@@ -587,6 +647,7 @@ void BulletRobot::updateSensors(double dt)
BulletRobot::LinkInfo BulletRobot::getLink( VirtualRobot::RobotNodePtr node )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
for (size_t i=0;i<links.size();i++)
{
if (links[i].nodeJoint == node || links[i].nodeJoint2 == node)
@@ -598,6 +659,7 @@ BulletRobot::LinkInfo BulletRobot::getLink( VirtualRobot::RobotNodePtr node )
std::vector<BulletRobot::LinkInfo> BulletRobot::getLinks( VirtualRobot::RobotNodePtr node )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
std::vector<BulletRobot::LinkInfo> result;
for (size_t i=0;i<links.size();i++)
{
@@ -609,6 +671,7 @@ std::vector<BulletRobot::LinkInfo> BulletRobot::getLinks( VirtualRobot::RobotNod
bool BulletRobot::hasLink( VirtualRobot::RobotNodePtr node )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
for (size_t i=0;i<links.size();i++)
{
if (links[i].nodeJoint == node || links[i].nodeJoint2 == node)
@@ -619,6 +682,7 @@ bool BulletRobot::hasLink( VirtualRobot::RobotNodePtr node )
void BulletRobot::actuateNode( VirtualRobot::RobotNodePtr node, double jointValue )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(node);
if (node->isRotationalJoint())
@@ -672,6 +736,7 @@ void BulletRobot::actuateNode( VirtualRobot::RobotNodePtr node, double jointValu
void BulletRobot::actuateNodeVel(RobotNodePtr node, double jointVelocity)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(node);
if (node->isRotationalJoint())
@@ -725,10 +790,11 @@ void BulletRobot::actuateNodeVel(RobotNodePtr node, double jointVelocity)
double BulletRobot::getJointAngle( VirtualRobot::RobotNodePtr rn )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(rn);
if (!hasLink(rn))
{
- VR_ERROR << "No link with node " << rn->getName() << endl;
+ //VR_ERROR << "No link with node " << rn->getName() << endl;
return 0.0f;
}
LinkInfo link = getLink(rn);
@@ -773,10 +839,11 @@ double BulletRobot::getJointAngle( VirtualRobot::RobotNodePtr rn )
double BulletRobot::getJointTargetSpeed( VirtualRobot::RobotNodePtr rn )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(rn);
if (!hasLink(rn))
{
- VR_ERROR << "No link with node " << rn->getName() << endl;
+ //VR_ERROR << "No link with node " << rn->getName() << endl;
return 0.0f;
}
LinkInfo link = getLink(rn);
@@ -804,10 +871,11 @@ double BulletRobot::getJointTargetSpeed( VirtualRobot::RobotNodePtr rn )
double BulletRobot::getJointSpeed( VirtualRobot::RobotNodePtr rn )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(rn);
if (!hasLink(rn))
{
- VR_ERROR << "No link with node " << rn->getName() << endl;
+ //VR_ERROR << "No link with node " << rn->getName() << endl;
return 0.0f;
}
LinkInfo link = getLink(rn);
@@ -831,6 +899,7 @@ double BulletRobot::getJointSpeed( VirtualRobot::RobotNodePtr rn )
double BulletRobot::getNodeTarget( VirtualRobot::RobotNodePtr node )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
return DynamicsRobot::getNodeTarget(node);
#else
@@ -841,12 +910,13 @@ double BulletRobot::getNodeTarget( VirtualRobot::RobotNodePtr node )
Eigen::Vector3f BulletRobot::getJointTorques(RobotNodePtr rn)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(rn);
Eigen::Vector3f result;
result.setZero();
if (!hasLink(rn))
{
- VR_ERROR << "No link with node " << rn->getName() << endl;
+ //VR_ERROR << "No link with node " << rn->getName() << endl;
return result;
}
LinkInfo link = getLink(rn);
@@ -862,10 +932,11 @@ Eigen::Vector3f BulletRobot::getJointTorques(RobotNodePtr rn)
double BulletRobot::getJointTorque(RobotNodePtr rn)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(rn);
if (!hasLink(rn))
{
- VR_ERROR << "No link with node " << rn->getName() << endl;
+ //VR_ERROR << "No link with node " << rn->getName() << endl;
return 0.0;
}
LinkInfo link = getLink(rn);
@@ -883,12 +954,13 @@ double BulletRobot::getJointTorque(RobotNodePtr rn)
Eigen::Vector3f BulletRobot::getJointForces(RobotNodePtr rn)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(rn);
Eigen::Vector3f result;
result.setZero();
if (!hasLink(rn))
{
- VR_ERROR << "No link with node " << rn->getName() << endl;
+ //VR_ERROR << "No link with node " << rn->getName() << endl;
return result;
}
LinkInfo link = getLink(rn);
@@ -904,6 +976,7 @@ Eigen::Vector3f BulletRobot::getJointForces(RobotNodePtr rn)
Eigen::Matrix4f BulletRobot::getComGlobal( VirtualRobot::RobotNodePtr rn )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
BulletObjectPtr bo = boost::dynamic_pointer_cast<BulletObject>(getDynamicsRobotNode(rn));
if (!bo)
{
@@ -915,6 +988,7 @@ Eigen::Matrix4f BulletRobot::getComGlobal( VirtualRobot::RobotNodePtr rn )
Eigen::Vector3f BulletRobot::getComGlobal( VirtualRobot::RobotNodeSetPtr set)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
Eigen::Vector3f com = Eigen::Vector3f::Zero();
double totalMass = 0.0;
for (unsigned int i = 0; i < set->getSize(); i++)
@@ -932,6 +1006,7 @@ Eigen::Vector3f BulletRobot::getComGlobal( VirtualRobot::RobotNodeSetPtr set)
Eigen::Vector3f BulletRobot::getComVelocityGlobal( VirtualRobot::RobotNodeSetPtr set)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
Eigen::Vector3f com = Eigen::Vector3f::Zero();
double totalMass = 0.0;
for (unsigned int i = 0; i < set->getSize(); i++)
@@ -939,16 +1014,32 @@ Eigen::Vector3f BulletRobot::getComVelocityGlobal( VirtualRobot::RobotNodeSetPtr
VirtualRobot::RobotNodePtr node = (*set)[i];
BulletObjectPtr bo = boost::dynamic_pointer_cast<BulletObject>(getDynamicsRobotNode(node));
Eigen::Vector3f vel = bo->getLinearVelocity();
+ if (boost::math::isnan(vel(0)) || boost::math::isnan(vel(1)) || boost::math::isnan(vel(2)))
+ {
+ VR_ERROR << "NAN result: getLinearVelocity:" << bo->getName() << ", i:" << i << endl;
+ node->print();
+ VR_ERROR << "BULLETOBJECT com:" << bo->getCom() << endl;
+ VR_ERROR << "BULLETOBJECT: ang vel:" << bo->getAngularVelocity() << endl;
+ VR_ERROR << "BULLETOBJECT:" << bo->getRigidBody()->getWorldTransform().getOrigin() << endl;
+
+
+ }
+
com += node->getMass() * vel;
totalMass += node->getMass();
}
-
- com *= float(1.0f/totalMass);
+ if (fabs(totalMass)<1e-5)
+ {
+ VR_ERROR << "Little mass: " << totalMass << ". Could not compute com velocity..." << endl;
+ }
+ else
+ com *= float(1.0f/totalMass);
return com;
}
void BulletRobot::setPoseNonActuatedRobotNodes()
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(robot);
std::vector<RobotNodePtr> rns = robot->getRobotNodes();
std::vector<RobotNodePtr> actuatedNodes;
@@ -995,6 +1086,7 @@ void BulletRobot::setPoseNonActuatedRobotNodes()
void BulletRobot::enableForceTorqueFeedback(const LinkInfo& link , bool enable)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
if (!link.joint->needsFeedback() && enable)
{
link.joint->enableFeedback(true);
@@ -1013,6 +1105,7 @@ void BulletRobot::enableForceTorqueFeedback(const LinkInfo& link , bool enable)
Eigen::VectorXf BulletRobot::getForceTorqueFeedbackA( const LinkInfo& link )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
Eigen::VectorXf r(6);
r.setZero();
if (!link.joint || !link.joint->needsFeedback())
@@ -1026,8 +1119,10 @@ Eigen::VectorXf BulletRobot::getForceTorqueFeedbackA( const LinkInfo& link )
r << feedback->m_appliedForceBodyA[0],feedback->m_appliedForceBodyA[1],feedback->m_appliedForceBodyA[2],feedback->m_appliedTorqueBodyA[0],feedback->m_appliedTorqueBodyA[1],feedback->m_appliedTorqueBodyA[2];
return r;
}
+
Eigen::VectorXf BulletRobot::getForceTorqueFeedbackB( const LinkInfo& link )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
Eigen::VectorXf r(6);
r.setZero();
if (!link.joint || !link.joint->needsFeedback())
@@ -1044,6 +1139,7 @@ Eigen::VectorXf BulletRobot::getForceTorqueFeedbackB( const LinkInfo& link )
Eigen::VectorXf BulletRobot::getJointForceTorqueGlobal(const BulletRobot::LinkInfo &link)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
Eigen::VectorXf ftA = getForceTorqueFeedbackA(link);
Eigen::VectorXf ftB = getForceTorqueFeedbackB(link);
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobotLogger.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobotLogger.h
index 0482600ef..0cd59da80 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobotLogger.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobotLogger.h
@@ -15,6 +15,7 @@ namespace SimDynamics
class SIMDYNAMICS_IMPORT_EXPORT BulletRobotLogger
{
public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
BulletRobotLogger(BulletEnginePtr engine,
const BulletRobotPtr robot,
const VirtualRobot::RobotNodeSetPtr& jointNodes,
@@ -27,6 +28,13 @@ public:
, timestamp(0.0f)
, logPath("")
{
+ int dof = jointNodes->getSize();
+ actualAngle.resize(dof);
+ targetAngle.resize(dof);
+ actualVelocity.resize(dof);
+ targetVelocity.resize(dof);
+ actualTorque.resize(dof);
+ actualForces.resize(3, dof);
engine->addExternalCallback(logCB, (void*) this);
}
@@ -49,21 +57,28 @@ public:
private:
const BulletRobotPtr robot;
- VirtualRobot::RobotNodeSetPtr jointNodes;
+ bool running;
+ VirtualRobot::RobotNodeSetPtr jointNodes;
VirtualRobot::RobotNodeSetPtr bodyNodes;
- std::vector<Eigen::VectorXf> targetAngleLog;
- std::vector<Eigen::VectorXf> targetVelocityLog;
- std::vector<Eigen::VectorXf> actualAngleLog;
- std::vector<Eigen::VectorXf> actualVelocityLog;
- std::vector<Eigen::VectorXf> actualJointTorquesLog;
- std::vector<Eigen::Matrix3Xf> actualJointForcesLog;
- std::vector<Eigen::Vector3f> actualCoMLog;
- std::vector<Eigen::Vector3f> actualCoMVelocityLog;
+ std::vector<Eigen::VectorXf, Eigen::aligned_allocator<Eigen::VectorXf> > targetAngleLog;
+ std::vector<Eigen::VectorXf, Eigen::aligned_allocator<Eigen::VectorXf> > targetVelocityLog;
+ std::vector<Eigen::VectorXf, Eigen::aligned_allocator<Eigen::VectorXf> > actualAngleLog;
+ std::vector<Eigen::VectorXf, Eigen::aligned_allocator<Eigen::VectorXf> > actualVelocityLog;
+ std::vector<Eigen::VectorXf, Eigen::aligned_allocator<Eigen::VectorXf> > actualJointTorquesLog;
+ std::vector<Eigen::Matrix3Xf, Eigen::aligned_allocator<Eigen::Matrix3Xf> > actualJointForcesLog;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > actualCoMLog;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > actualCoMVelocityLog;
std::vector<double> timestamps;
- double timestamp;
- bool running;
int max_samples;
- std::string logPath;
+ double timestamp;
+ std::string logPath;
+
+ Eigen::VectorXf actualAngle;
+ Eigen::VectorXf targetAngle;
+ Eigen::VectorXf actualVelocity;
+ Eigen::VectorXf targetVelocity;
+ Eigen::VectorXf actualTorque;
+ Eigen::Matrix3Xf actualForces;
static void logCB(void* data, btScalar dt);
void log(btScalar dt);
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
index aa0d92a8f..def3ee37a 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
@@ -4,6 +4,7 @@
#include "../../DynamicsWorld.h"
#include <boost/pointer_cast.hpp>
+#include <boost/math/special_functions/fpclassify.hpp>
using namespace VirtualRobot;
@@ -60,6 +61,12 @@ void SimoxMotionState::setWorldTransform(const btTransform& worldPose)
// _transform is the Bullet pose, used in getWorldTransform().
_transform = worldPose; // com position
+#ifdef _DEBUG
+ if (boost::math::isnan(_transform.getOrigin().x()) || boost::math::isnan(_transform.getOrigin().y()) || boost::math::isnan(_transform.getOrigin().z()))
+ {
+ VR_ERROR << "NAN transform!!!" << endl;
+ }
+#endif
m_graphicsWorldTrans = _transform; // this is used for debug drawing
_graphicsTransfrom = _transform;
//_graphicsTransfrom.getOrigin();// -= _comOffset.getOrigin(); // com adjusted
@@ -123,6 +130,12 @@ void SimoxMotionState::setGlobalPoseSimox( const Eigen::Matrix4f& worldPose )
if (links[i].nodeJoint)
{
float ja = float(bdr->getJointAngle(links[i].nodeJoint));
+#ifdef _DEBUG
+ if (boost::math::isnan(ja))
+ {
+ VR_ERROR << "NAN !!!" << endl;
+ }
+#endif
// we can update the joint value via an RobotNodeActuator
RobotNodeActuatorPtr rna (new RobotNodeActuator(links[i].nodeJoint));
rna->updateJointAngle(ja);
diff --git a/SimDynamics/DynamicsEngine/DynamicsEngine.cpp b/SimDynamics/DynamicsEngine/DynamicsEngine.cpp
index 006f23eb1..dc15f6a0b 100644
--- a/SimDynamics/DynamicsEngine/DynamicsEngine.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsEngine.cpp
@@ -4,12 +4,16 @@
namespace SimDynamics {
-DynamicsEngine::DynamicsEngine()
+DynamicsEngine::DynamicsEngine(boost::shared_ptr <boost::recursive_mutex> engineMutex)
{
floorPos.setZero();
floorUp.setZero();
floorDepthMM = 500.0f;
floorExtendMM = 50000.0f;
+ if (engineMutex)
+ engineMutexPtr = engineMutex;
+ else
+ engineMutexPtr.reset(new boost::recursive_mutex());
}
DynamicsEngine::~DynamicsEngine()
@@ -20,7 +24,7 @@ DynamicsEngine::~DynamicsEngine()
Eigen::Vector3f DynamicsEngine::getGravity()
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
return dynamicsConfig->gravity;
}
@@ -30,22 +34,28 @@ bool DynamicsEngine::init( DynamicsEngineConfigPtr config )
dynamicsConfig = config;
else
dynamicsConfig.reset(new DynamicsEngineConfig());
- return true;
+ return true;
+}
+
+void DynamicsEngine::setMutex(boost::shared_ptr<boost::recursive_mutex> engineMutex)
+{
+ engineMutexPtr = engineMutex;
}
bool DynamicsEngine::addObject( DynamicsObjectPtr o )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
if (find(objects.begin(), objects.end(), o) == objects.end())
{
objects.push_back(o);
}
+ o->setMutex(engineMutexPtr);
return true;
}
bool DynamicsEngine::removeObject( DynamicsObjectPtr o )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
std::vector<DynamicsObjectPtr>::iterator it = find(objects.begin(), objects.end(), o);
if (it == objects.end())
return false;
@@ -56,14 +66,14 @@ bool DynamicsEngine::removeObject( DynamicsObjectPtr o )
void DynamicsEngine::createFloorPlane( const Eigen::Vector3f &pos, const Eigen::Vector3f &up )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
floorPos = pos;
floorUp = up;
}
bool DynamicsEngine::addRobot( DynamicsRobotPtr r )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
if (find(robots.begin(), robots.end(), r) == robots.end())
{
for (size_t i=0;i<robots.size();i++)
@@ -76,12 +86,13 @@ bool DynamicsEngine::addRobot( DynamicsRobotPtr r )
}
robots.push_back(r);
}
+ r->setMutex(engineMutexPtr);
return true;
}
bool DynamicsEngine::removeRobot( DynamicsRobotPtr r )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
std::vector<DynamicsRobotPtr>::iterator it = find(robots.begin(), robots.end(), r);
if (it == robots.end())
return false;
@@ -92,7 +103,7 @@ bool DynamicsEngine::removeRobot( DynamicsRobotPtr r )
void DynamicsEngine::disableCollision( DynamicsObject* o1, DynamicsObject* o2 )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
std::vector<DynamicsObject*>::iterator i1 = find(collisionDisabled[o1].begin(),collisionDisabled[o1].end(),o2);
if (i1==collisionDisabled[o1].end())
{
@@ -107,7 +118,7 @@ void DynamicsEngine::disableCollision( DynamicsObject* o1, DynamicsObject* o2 )
void DynamicsEngine::disableCollision( DynamicsObject* o1 )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
std::vector<DynamicsObject*>::iterator i1 = find(collisionToAllDisabled.begin(),collisionToAllDisabled.end(),o1);
if (i1 == collisionToAllDisabled.end())
collisionToAllDisabled.push_back(o1);
@@ -115,7 +126,7 @@ void DynamicsEngine::disableCollision( DynamicsObject* o1 )
void DynamicsEngine::enableCollision( DynamicsObject* o1, DynamicsObject* o2 )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
//if (o1 < o2)
//{
std::vector<DynamicsObject*>::iterator i1 = find(collisionDisabled[o1].begin(),collisionDisabled[o1].end(),o2);
@@ -135,7 +146,7 @@ void DynamicsEngine::enableCollision( DynamicsObject* o1, DynamicsObject* o2 )
void DynamicsEngine::enableCollision( DynamicsObject* o1 )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
std::vector<DynamicsObject*>::iterator i1 = find(collisionToAllDisabled.begin(),collisionToAllDisabled.end(),o1);
if (i1 != collisionToAllDisabled.end())
collisionToAllDisabled.erase(i1);
@@ -143,7 +154,7 @@ void DynamicsEngine::enableCollision( DynamicsObject* o1 )
bool DynamicsEngine::checkCollisionEnabled( DynamicsObject* o1 )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
std::vector<DynamicsObject*>::iterator i1 = find(collisionToAllDisabled.begin(),collisionToAllDisabled.end(),o1);
return (i1 == collisionToAllDisabled.end());
}
@@ -158,7 +169,7 @@ void DynamicsEngine::getFloorInfo(Eigen::Vector3f &floorPos, Eigen::Vector3f &fl
bool DynamicsEngine::checkCollisionEnabled( DynamicsObject* o1, DynamicsObject* o2 )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
if (!checkCollisionEnabled(o1))
return false;
if (!checkCollisionEnabled(o2))
@@ -180,7 +191,7 @@ bool DynamicsEngine::checkCollisionEnabled( DynamicsObject* o1, DynamicsObject*
void DynamicsEngine::resetCollisions( DynamicsObject* o )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
collisionDisabled.erase(o);
std::map < DynamicsObject*, std::vector<DynamicsObject*> >::iterator i1 = collisionDisabled.begin();
while (i1 != collisionDisabled.end())
@@ -197,26 +208,26 @@ void DynamicsEngine::resetCollisions( DynamicsObject* o )
std::vector<DynamicsRobotPtr> DynamicsEngine::getRobots()
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
return robots;
}
std::vector<DynamicsObjectPtr> DynamicsEngine::getObjects()
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
return objects;
}
std::vector<DynamicsEngine::DynamicsContactInfo> DynamicsEngine::getContacts()
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
std::vector<DynamicsEngine::DynamicsContactInfo> result;
return result;
}
SimDynamics::DynamicsRobotPtr DynamicsEngine::getRobot( VirtualRobot::RobotPtr r )
{
- boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
for (size_t i=0;i<robots.size();i++)
{
if (robots[i]->getRobot() == r)
@@ -227,4 +238,10 @@ SimDynamics::DynamicsRobotPtr DynamicsEngine::getRobot( VirtualRobot::RobotPtr r
return DynamicsRobotPtr();
}
+DynamicsEngine::MutexLockPtr DynamicsEngine::getScopedLock()
+{
+ boost::shared_ptr< boost::recursive_mutex::scoped_lock > scoped_lock(new boost::recursive_mutex::scoped_lock(*engineMutexPtr));
+ return scoped_lock;
+}
+
} // namespace SimDynamics
diff --git a/SimDynamics/DynamicsEngine/DynamicsEngine.h b/SimDynamics/DynamicsEngine/DynamicsEngine.h
index c3d7717c5..d5b8bbd1c 100644
--- a/SimDynamics/DynamicsEngine/DynamicsEngine.h
+++ b/SimDynamics/DynamicsEngine/DynamicsEngine.h
@@ -59,10 +59,11 @@ class SIMDYNAMICS_IMPORT_EXPORT DynamicsEngine
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- /*!
- Constructor
+ /*!
+ Constructor
+ \param engineMutex Optionally, all engine access methods can be protected by an external mutex. If not set, an internal mutex is creeated.
*/
- DynamicsEngine();
+ DynamicsEngine(boost::shared_ptr <boost::recursive_mutex> engineMutex = boost::shared_ptr<boost::recursive_mutex>());
/*!
*/
@@ -73,6 +74,8 @@ public:
*/
virtual bool init(DynamicsEngineConfigPtr config);
+ void setMutex(boost::shared_ptr <boost::recursive_mutex> engineMutex);
+
Eigen::Vector3f getGravity();
virtual bool addObject(DynamicsObjectPtr o);
@@ -152,6 +155,23 @@ public:
*/
virtual DynamicsRobotPtr getRobot(VirtualRobot::RobotPtr r);
+ typedef boost::shared_ptr< boost::recursive_mutex::scoped_lock > MutexLockPtr;
+
+ /*!
+ This lock can be used to protect data access. It locks the mutex until deletion.
+
+ Exemplary usage:
+ {
+ MutexLockPtr lock = engine->getScopedLock();
+ // now the mutex is locked
+
+ // access data
+ // ...
+
+ } // end of scope -> lock gets deleted and mutex is released automatically
+ */
+ MutexLockPtr getScopedLock();
+
protected:
DynamicsEngineConfigPtr dynamicsConfig;
@@ -168,7 +188,7 @@ protected:
double floorExtendMM;
double floorDepthMM;
- boost::recursive_mutex engineMutex;
+ boost::shared_ptr <boost::recursive_mutex> engineMutexPtr;
};
diff --git a/SimDynamics/DynamicsEngine/DynamicsObject.cpp b/SimDynamics/DynamicsEngine/DynamicsObject.cpp
index f9a7116a8..8f8491c4b 100644
--- a/SimDynamics/DynamicsEngine/DynamicsObject.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsObject.cpp
@@ -8,6 +8,7 @@ DynamicsObject::DynamicsObject(VirtualRobot::SceneObjectPtr o)
{
THROW_VR_EXCEPTION_IF(!o,"NULL object");
sceneObject = o;
+ engineMutexPtr.reset(new boost::recursive_mutex()); // may be overwritten by another mutex!
}
DynamicsObject::~DynamicsObject()
@@ -16,6 +17,7 @@ DynamicsObject::~DynamicsObject()
std::string DynamicsObject::getName() const
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
return sceneObject->getName();
}
@@ -26,6 +28,7 @@ VirtualRobot::SceneObject::Physics::SimulationType DynamicsObject::getSimType()
void DynamicsObject::setPose( const Eigen::Matrix4f &pose )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
if (sceneObject->getSimulationType()==VirtualRobot::SceneObject::Physics::eStatic)
{
VR_ERROR << "Could not move static object, aborting..." << endl;
@@ -36,6 +39,7 @@ void DynamicsObject::setPose( const Eigen::Matrix4f &pose )
void DynamicsObject::setPosition( const Eigen::Vector3f &posMM )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
Eigen::Matrix4f pose = sceneObject->getGlobalPoseVisualization();
pose.block(0,3,3,1) = posMM;
setPose(pose);
@@ -76,10 +80,16 @@ void DynamicsObject::applyTorque(const Eigen::Vector3f &torque)
}
+void DynamicsObject::setMutex(boost::shared_ptr<boost::recursive_mutex> engineMutexPtr)
+{
+ this->engineMutexPtr = engineMutexPtr;
+}
+
void DynamicsObject::setSimType( VirtualRobot::SceneObject::Physics::SimulationType s )
{
sceneObject->setSimulationType(s);
}
+
} // namespace SimDynamics
diff --git a/SimDynamics/DynamicsEngine/DynamicsObject.h b/SimDynamics/DynamicsEngine/DynamicsObject.h
index ef4ccedcf..de054c7ae 100644
--- a/SimDynamics/DynamicsEngine/DynamicsObject.h
+++ b/SimDynamics/DynamicsEngine/DynamicsObject.h
@@ -25,6 +25,7 @@
#include "../SimDynamics.h"
#include <VirtualRobot/SceneObject.h>
+#include <boost/thread/recursive_mutex.hpp>
namespace SimDynamics
{
@@ -82,12 +83,18 @@ public:
*/
virtual void applyTorque(const Eigen::Vector3f &torque);
+ //! If set, all actions are protected with this mutex
+ virtual void setMutex(boost::shared_ptr <boost::recursive_mutex> engineMutexPtr);
+
virtual void setSimType(VirtualRobot::SceneObject::Physics::SimulationType s);
protected:
VirtualRobot::SceneObjectPtr sceneObject;
+
+ boost::shared_ptr <boost::recursive_mutex> engineMutexPtr;
+
};
typedef boost::shared_ptr<DynamicsObject> DynamicsObjectPtr;
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
index 66a836347..4d5852962 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
@@ -10,7 +10,7 @@ DynamicsRobot::DynamicsRobot(VirtualRobot::RobotPtr rob)
THROW_VR_EXCEPTION_IF(!rob,"NULL object");
robot = rob;
sensors = rob->getSensors();
-
+ engineMutexPtr.reset(new boost::recursive_mutex()); // may be overwritten by another mutex!
}
DynamicsRobot::~DynamicsRobot()
@@ -19,6 +19,7 @@ DynamicsRobot::~DynamicsRobot()
std::string DynamicsRobot::getName() const
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
return robot->getName();
}
@@ -29,6 +30,7 @@ void DynamicsRobot::ensureKinematicConstraints()
void DynamicsRobot::createDynamicsNode( VirtualRobot::RobotNodePtr node )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(node);
// check if already created
@@ -44,6 +46,7 @@ void DynamicsRobot::createDynamicsNode( VirtualRobot::RobotNodePtr node )
std::vector<DynamicsObjectPtr> DynamicsRobot::getDynamicsRobotNodes()
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
std::map<VirtualRobot::RobotNodePtr, DynamicsObjectPtr>::iterator it = dynamicRobotNodes.begin();
std::vector<DynamicsObjectPtr> res;
while (it != dynamicRobotNodes.end())
@@ -56,6 +59,7 @@ std::vector<DynamicsObjectPtr> DynamicsRobot::getDynamicsRobotNodes()
bool DynamicsRobot::hasDynamicsRobotNode( VirtualRobot::RobotNodePtr node )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
if (dynamicRobotNodes.find(node) == dynamicRobotNodes.end())
return false;
return true;
@@ -63,6 +67,7 @@ bool DynamicsRobot::hasDynamicsRobotNode( VirtualRobot::RobotNodePtr node )
DynamicsObjectPtr DynamicsRobot::getDynamicsRobotNode( VirtualRobot::RobotNodePtr node )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(node);
if (dynamicRobotNodes.find(node) == dynamicRobotNodes.end())
return DynamicsObjectPtr();
@@ -71,6 +76,7 @@ DynamicsObjectPtr DynamicsRobot::getDynamicsRobotNode( VirtualRobot::RobotNodePt
void DynamicsRobot::actuateNode(const std::string &node, double jointValue )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(robot);
VR_ASSERT(robot->hasRobotNode(node));
actuateNode(robot->getRobotNode(node),jointValue);
@@ -78,6 +84,7 @@ void DynamicsRobot::actuateNode(const std::string &node, double jointValue )
void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, double jointValue )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(robot);
VR_ASSERT(node);
VR_ASSERT(robot->hasRobotNode(node));
@@ -107,6 +114,7 @@ void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, double jointVa
void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, double jointValue , double jointVelocity)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(robot);
VR_ASSERT(node);
VR_ASSERT(robot->hasRobotNode(node));
@@ -131,6 +139,7 @@ void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, double jointVa
void DynamicsRobot::actuateNodeVel(const std::string &node, double jointVelocity )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(robot);
VR_ASSERT(robot->hasRobotNode(node));
@@ -139,6 +148,7 @@ void DynamicsRobot::actuateNodeVel(const std::string &node, double jointVelocity
void DynamicsRobot::actuateNodeVel( VirtualRobot::RobotNodePtr node, double jointVelocity )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(robot);
VR_ASSERT(node);
VR_ASSERT(robot->hasRobotNode(node));
@@ -166,6 +176,7 @@ void DynamicsRobot::actuateNodeVel( VirtualRobot::RobotNodePtr node, double join
void DynamicsRobot::actuateNodeTorque(const std::string &node, double jointTorque )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(robot);
VR_ASSERT(robot->hasRobotNode(node));
actuateNodeTorque(robot->getRobotNode(node),jointTorque);
@@ -173,6 +184,7 @@ void DynamicsRobot::actuateNodeTorque(const std::string &node, double jointTorqu
void DynamicsRobot::actuateNodeTorque( VirtualRobot::RobotNodePtr node, double jointTorque )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(robot);
VR_ASSERT(node);
VR_ASSERT(robot->hasRobotNode(node));
@@ -201,13 +213,16 @@ void DynamicsRobot::actuateNodeTorque( VirtualRobot::RobotNodePtr node, double j
void DynamicsRobot::disableNodeActuation( VirtualRobot::RobotNodePtr node )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
if (actuationTargets.find(node) != actuationTargets.end())
{
actuationTargets.erase(node);
}
}
+
void DynamicsRobot::enableActuation(ActuationMode mode)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
std::map<VirtualRobot::RobotNodePtr, robotNodeActuationTarget>::iterator it = actuationTargets.begin();
while (it!=actuationTargets.end())
{
@@ -215,8 +230,10 @@ void DynamicsRobot::enableActuation(ActuationMode mode)
it++;
}
}
+
void DynamicsRobot::disableActuation()
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
std::map<VirtualRobot::RobotNodePtr, robotNodeActuationTarget>::iterator it = actuationTargets.begin();
while (it!=actuationTargets.end())
{
@@ -231,6 +248,7 @@ void DynamicsRobot::actuateJoints(double dt)
bool DynamicsRobot::isNodeActuated( VirtualRobot::RobotNodePtr node )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(node);
if (actuationTargets.find(node) == actuationTargets.end())
return false;
@@ -239,6 +257,7 @@ bool DynamicsRobot::isNodeActuated( VirtualRobot::RobotNodePtr node )
double DynamicsRobot::getNodeTarget( VirtualRobot::RobotNodePtr node )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
VR_ASSERT(node);
if (actuationTargets.find(node) == actuationTargets.end())
return 0.0f;
@@ -263,6 +282,7 @@ double DynamicsRobot::getJointTargetSpeed( VirtualRobot::RobotNodePtr rn )
Eigen::Matrix4f DynamicsRobot::getComGlobal( VirtualRobot::RobotNodePtr rn )
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
Eigen::Matrix4f com = Eigen::Matrix4f::Identity();
com.block(0,3,3,1) = rn->getCoMLocal();
com = rn->getGlobalPoseVisualization()*com;
@@ -271,6 +291,7 @@ Eigen::Matrix4f DynamicsRobot::getComGlobal( VirtualRobot::RobotNodePtr rn )
void DynamicsRobot::setGlobalPose(Eigen::Matrix4f &gp)
{
+ boost::recursive_mutex::scoped_lock scoped_lock(*engineMutexPtr);
Eigen::Matrix4f currentPose = robot->getGlobalPose();
Eigen::Matrix4f delta = gp * currentPose.inverse();
@@ -284,6 +305,11 @@ void DynamicsRobot::setGlobalPose(Eigen::Matrix4f &gp)
}
}
+void DynamicsRobot::setMutex(boost::shared_ptr<boost::recursive_mutex> engineMutexPtr)
+{
+ this->engineMutexPtr = engineMutexPtr;
+}
+
std::map<VirtualRobot::RobotNodePtr, VelocityMotorController>& DynamicsRobot::getControllers()
{
return actuationControllers;
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.h b/SimDynamics/DynamicsEngine/DynamicsRobot.h
index 1675466d9..729c08e93 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.h
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.h
@@ -100,6 +100,9 @@ public:
virtual void setGlobalPose(Eigen::Matrix4f &gp);
+ //! If set, all actions are protected with this mutex
+ virtual void setMutex(boost::shared_ptr <boost::recursive_mutex> engineMutexPtr);
+
//! can be used to access the internal controllers
std::map<VirtualRobot::RobotNodePtr, VelocityMotorController>& getControllers();
@@ -134,6 +137,8 @@ protected:
std::vector<VirtualRobot::RobotNodePtr> robotNodes;
std::map<VirtualRobot::RobotNodePtr, DynamicsObjectPtr> dynamicRobotNodes;
+
+ boost::shared_ptr <boost::recursive_mutex> engineMutexPtr;
};
typedef boost::shared_ptr<DynamicsRobot> DynamicsRobotPtr;
diff --git a/SimDynamics/DynamicsEngine/DynamicsUtils.cpp b/SimDynamics/DynamicsEngine/DynamicsUtils.cpp
index 8b009b14a..ed6353a30 100644
--- a/SimDynamics/DynamicsEngine/DynamicsUtils.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsUtils.cpp
@@ -110,12 +110,17 @@ double TorqueMotorController::update(double positionError, double velocityError,
}
VelocityMotorController::VelocityMotorController(double maxVelocity, double maxAcceleration)
-: positionController(80.0, 10.0, 0.8)
+ : positionController(15.0, 0.0, 0.0)
+, velocityController(15.0, 0.0, 0.0)
, maxVelocity(maxVelocity)
, maxAcceleration(maxAcceleration)
, velocity(0)
{
}
+void VelocityMotorController::setCurrentVelocity(double vel)
+{
+ velocity = vel;
+}
double VelocityMotorController::update(double positionError, double targetVelocity, ActuationMode actuation, double dt)
{
@@ -131,7 +136,10 @@ double VelocityMotorController::update(double positionError, double targetVeloci
else if (actuation.modes.position)
output = posUpdate;
else if (actuation.modes.velocity)
- output = targetVelocity;
+ {
+ double velError = targetVelocity - velocity;
+ output = velocityController.update(velError, dt);
+ }
if (maxVelocity > 0.0 && fabs(output) > maxVelocity)
{
@@ -170,4 +178,7 @@ void VelocityMotorController::debug()
{
positionController.debug();
}
+
+
+
}
diff --git a/SimDynamics/DynamicsEngine/DynamicsUtils.h b/SimDynamics/DynamicsEngine/DynamicsUtils.h
index 066306d3b..9501e7f30 100644
--- a/SimDynamics/DynamicsEngine/DynamicsUtils.h
+++ b/SimDynamics/DynamicsEngine/DynamicsUtils.h
@@ -83,11 +83,12 @@ private:
*
* Note: Torque is ignored. This controler returns *velocities*.
*
+ *
* Position only:
* position error --> [PID] --> joint
*
* Velocity only:
- * target velocity --> joint
+ * velocity error -> [PID] --> joint
*
* Position + Velocity:
* target velocity
@@ -102,20 +103,26 @@ public:
VelocityMotorController(const PIDController& positionController);
- double update(double positionError, double targetVelocity, ActuationMode actuation, double dt);
+ void setCurrentVelocity(double vel);
- void reset();
+ double update(double positionError, double targetVelocity, ActuationMode actuation, double dt);
+
+ void reset();
+ //! set new p,i,d values for position controller
void reset(double pid_pos_gainP, double pid_pos_gainI, double pid_pos_gainD);
void debug();
void getPosPID(double &storeP, double &storeI, double &storeD);
private:
- PIDController positionController;
- double maxVelocity;
+ PIDController positionController;
+ PIDController velocityController;
+ double maxVelocity;
double maxAcceleration;
double velocity;
};
+
}
+
#endif
diff --git a/VirtualRobot/IK/DifferentialIK.cpp b/VirtualRobot/IK/DifferentialIK.cpp
index 182be33ef..feba40a3d 100644
--- a/VirtualRobot/IK/DifferentialIK.cpp
+++ b/VirtualRobot/IK/DifferentialIK.cpp
@@ -511,7 +511,15 @@ bool DifferentialIK::checkTolerances()
bool result = true;
for (size_t i=0; i<tcp_set.size();i++){
SceneObjectPtr tcp = tcp_set[i];
- if (getErrorPosition(tcp) > tolerancePosition[tcp] || getErrorRotation(tcp)>toleranceRotation[tcp])
+ float currentErrorPos = getErrorPosition(tcp);
+ float maxErrorPos = tolerancePosition[tcp];
+ float currentErrorRot = getErrorRotation(tcp);
+ float maxErrorRot = toleranceRotation[tcp];
+ if (verbose)
+ {
+ VR_INFO << "TCP " << tcp->getName() << ", errPos:" << currentErrorPos << ", errRot:" << currentErrorRot << ", maxErrPos:" << maxErrorPos << ", maxErrorRot:" << maxErrorRot << endl;
+ }
+ if (currentErrorPos > maxErrorPos || currentErrorRot>maxErrorRot)
{
result = false;
//break;
--
GitLab