diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp
index 908fd30b55076bcfc7331a17179a75f6bb3ddc41..1a92a4d2fb73badbeae2c6e884499f0334669596 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp
@@ -22,7 +22,7 @@ BulletCoinQtViewer::BulletCoinQtViewer(DynamicsWorldPtr world)
bulletMaxSubSteps = 1;
enablePhysicsUpdates = true;
- const float TIMER_MS = 5.0f;
+ const double TIMER_MS = 5.0f;
SIMDYNAMICS_ASSERT(world);
@@ -122,8 +122,8 @@ void BulletCoinQtViewer::initSceneGraph( QFrame* embedViewer, SoNode* scene )
// better grid visu
Eigen::Vector3f floorPos;
Eigen::Vector3f floorUp;
- float floorExtendMM;
- float floorDepthMM;
+ double floorExtendMM;
+ double floorDepthMM;
bulletEngine->getFloorInfo(floorPos,floorUp,floorExtendMM,floorDepthMM);
SoNode * n = (SoNode*)CoinVisualizationFactory::CreatePlaneVisualization(floorPos,floorUp,floorExtendMM,0.0f);
if (n)
@@ -150,14 +150,14 @@ void BulletCoinQtViewer::stepPhysics()
boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
//simple dynamics world doesn't handle fixed-time-stepping
- float ms = getDeltaTimeMicroseconds();
+ double ms = getDeltaTimeMicroseconds();
if (bulletEngine)
{
bulletEngine->activateAllObjects(); // avoid sleeping objects
// Commented out: This is now handled by Bullet (bulletMaxSubSteps * bulletTimeStepMsec is the maximal duration of a frame)
- /* float minFPS = 1000000.f/40.f; // Don't use 60 Hz (cannot be reached due to Vsync)
+ /* double minFPS = 1000000.f/40.f; // Don't use 60 Hz (cannot be reached due to Vsync)
if (ms > minFPS) {
VR_INFO << "Slow frame (" << ms << "us elapsed)! Limiting elapsed time (losing realtime capabilities for this frame)." << endl;
ms = minFPS;
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
index 0d83fa838e8769e7bf0ab5dbee5e22551c0265d8..2ee74b05346343eded5cf56b278494d7d8b75303 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
@@ -230,11 +230,11 @@ void BulletEngine::createFloorPlane( const Eigen::Vector3f &pos, const Eigen::Ve
{
boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
DynamicsEngine::createFloorPlane(pos,up);
- float size = floorExtendMM;//50000.0f; // mm
- float sizeSmall = floorDepthMM; 500.0f;
- float w = size;
- float h = size;
- float d = sizeSmall;
+ double size = floorExtendMM;//50000.0f; // mm
+ double sizeSmall = floorDepthMM; 500.0f;
+ double w = size;
+ double h = size;
+ double d = sizeSmall;
if (up(1) == 0 && up(2) == 0)
{
w = sizeSmall;
@@ -288,7 +288,7 @@ Eigen::Matrix4f BulletEngine::getRotMatrix(const btMatrix3x3 &pose)
btTransform BulletEngine::getPoseBullet( const Eigen::Matrix4f &pose, bool scaling )
{
btTransform res;
- float sc = 1.0f;
+ double sc = 1.0f;
if (scaling && DynamicsWorld::convertMM2M)
sc = 0.001f; // mm -> m
btVector3 pos(pose(0,3)*sc,pose(1,3)*sc,pose(2,3)*sc);
@@ -302,7 +302,7 @@ btTransform BulletEngine::getPoseBullet( const Eigen::Matrix4f &pose, bool scali
Eigen::Matrix4f BulletEngine::getPoseEigen( const btTransform &pose, bool scaling )
{
- float sc = 1.0f;
+ double sc = 1.0f;
if (scaling && DynamicsWorld::convertMM2M)
sc = 1000.0f; // m -> mm
@@ -323,7 +323,7 @@ Eigen::Matrix4f BulletEngine::getPoseEigen( const btTransform &pose, bool scalin
btVector3 BulletEngine::getVecBullet( const Eigen::Vector3f &vec, bool scaling )
{
btTransform res;
- float sc = 1.0f;
+ double sc = 1.0f;
if (scaling && DynamicsWorld::convertMM2M)
sc = 0.001f; // mm -> m
btVector3 pos(vec(0)*sc,vec(1)*sc,vec(2)*sc);
@@ -332,7 +332,7 @@ btVector3 BulletEngine::getVecBullet( const Eigen::Vector3f &vec, bool scaling )
Eigen::Vector3f BulletEngine::getVecEigen( const btVector3 &vec, bool scaling )
{
- float sc = 1.0f;
+ double sc = 1.0f;
if (scaling && DynamicsWorld::convertMM2M)
sc = 1000.0f; // m -> mm
@@ -551,7 +551,7 @@ std::vector<DynamicsEngine::DynamicsContactInfo> BulletEngine::getContacts()
return result;
}
-void BulletEngine::stepSimulation( float dt, int maxSubSteps, float fixedTimeStep )
+void BulletEngine::stepSimulation( double dt, int maxSubSteps, double fixedTimeStep )
{
boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
dynamicsWorld->stepSimulation(dt, maxSubSteps, fixedTimeStep);
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
index 30b230b784d37a0bd2ca97dac0cb5ad3688cd1bc..3aad49983ed5868e0907e0327e1c47b0445988c2 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
@@ -114,7 +114,7 @@ public:
virtual void createFloorPlane(const Eigen::Vector3f &pos, const Eigen::Vector3f &up);
- virtual void stepSimulation(float dt, int maxSubSteps, float fixedTimeStep);
+ virtual void stepSimulation(double dt, int maxSubSteps, double fixedTimeStep);
btDynamicsWorld* getBulletWorld();
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
index 92c92045df132a9ee0d4a679d601476791a2b355..3cbb429443fb9e21d2007514c78eed5f6be26835 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
@@ -21,7 +21,7 @@ namespace SimDynamics {
BulletObject::BulletObject(VirtualRobot::SceneObjectPtr o)
: DynamicsObject(o)
{
- float interatiaFactor = 1.0f;
+ double interatiaFactor = 1.0f;
#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
interatiaFactor = 5.0f;
#endif
@@ -131,7 +131,7 @@ btConvexHullShape* BulletObject::createConvexHullShape(VirtualRobot::TriMeshMode
comLoc = (sceneObject->getGlobalPoseVisualization().inverse()*comLoc);
com = comLoc.block(0,3,3,1);
- float sc = 1.0f;
+ double sc = 1.0f;
if (DynamicsWorld::convertMM2M)
sc = 0.001f;
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp
index 4576b8896145b99c54a79622a0dce55def0d4bc5..c4cb3fa9ed3f138aff2f31b48bf6f4424a38dd73 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp
@@ -65,9 +65,9 @@ void BulletOpenGLViewer::clientMoveAndDisplay()
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
//simple dynamics world doesn't handle fixed-time-stepping
- float ms = getDeltaTimeMicroseconds();
+ double ms = getDeltaTimeMicroseconds();
- float minFPS = 1000000.f/60.f;
+ double minFPS = 1000000.f/60.f;
if (ms > minFPS)
ms = minFPS;
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index e2779c664759efc71ea337db70e0afffe0489c8d..c074cd22a7d3d9189754ecce9b4968a27258c220 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -110,14 +110,14 @@ void BulletRobot::buildBulletModels(bool enableJointMotors)
Eigen::Matrix4f p1 = joint->getGlobalPoseJoint();
Eigen::Matrix4f p2 = joint2->getGlobalPoseJoint();
- float d = (p1.block(0,3,3,1) - p2.block(0,3,3,1)).norm();
+ double d = (p1.block(0,3,3,1) - p2.block(0,3,3,1)).norm();
THROW_VR_EXCEPTION_IF( (d>1e-6), "Could not create hinge2 joint: Joint coord systems must be located at the same position:" << joint->getName() << ", " << joint2->getName());
RobotNodeRevolutePtr rev1 = boost::dynamic_pointer_cast<RobotNodeRevolute>(joint);
RobotNodeRevolutePtr rev2 = boost::dynamic_pointer_cast<RobotNodeRevolute>(joint2);
THROW_VR_EXCEPTION_IF( !rev1 || !rev2 , "Could not create hinge2 joint: Joints must be revolute nodes:" << joint->getName() << ", " << joint2->getName());
Eigen::Vector3f ax1 = rev1->getJointRotationAxis();
Eigen::Vector3f ax2 = rev2->getJointRotationAxis();
- float ang = MathTools::getAngle(ax1,ax2);
+ double ang = MathTools::getAngle(ax1,ax2);
THROW_VR_EXCEPTION_IF( fabs(fabs(ang)-M_PI_2) > 1e-6, "Could not create hinge2 joint: Joint axes must be orthogonal to each other:" << joint->getName() << ", " << joint2->getName());
}
}
@@ -372,7 +372,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr bodyA, VirtualRobot::Ro
boost::shared_ptr<btTypedConstraint> jointbt;
- float vr2bulletOffset = 0.0f;
+ double vr2bulletOffset = 0.0f;
THROW_VR_EXCEPTION_IF (joint->isTranslationalJoint(), "Translational joints nyi...");
if (joint->isRotationalJoint())
@@ -387,7 +387,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr bodyA, VirtualRobot::Ro
tmpGpJoint.block(0,3,3,1).setZero(); // coordSystemJoint
Eigen::Vector4f axisGlobal = tmpGpJoint * axisLocalJoint;
- float limMin,limMax;
+ double limMin,limMax;
limMin = joint->getJointLimitLo();
limMax = joint->getJointLimitHi();
@@ -412,7 +412,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr bodyA, VirtualRobot::Ro
btVector3 axis2 = BulletEngine::getVecBullet(axisGlobal2.head(3),false);
btVector3 pivot = BulletEngine::getVecBullet(anchorPointGlobal.block(0,3,3,1));
boost::shared_ptr<btUniversalConstraint> hinge2(new btUniversalConstraint(*btBody1, *btBody2, pivot, axis1, axis2));
- float limMin2,limMax2;
+ double limMin2,limMax2;
limMin2 = joint2->getJointLimitLo();
limMax2 = joint2->getJointLimitHi();
hinge2->setLowerLimit(btScalar(limMin),btScalar(limMin2));
@@ -610,7 +610,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
boost::shared_ptr<btTypedConstraint> joint;
- float vr2bulletOffset = 0.0f;
+ double vr2bulletOffset = 0.0f;
if (node2->isTranslationalJoint())
{
@@ -654,7 +654,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
btVector3 pivot2 = BulletEngine::getVecBullet(anchor_inNode2.block(0,3,3,1));
- float limMin,limMax;
+ double limMin,limMax;
limMin = node2->getJointLimitLo();
limMax = node2->getJointLimitHi();
@@ -664,7 +664,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
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;
+ double ang1,ang2;
Eigen::Vector3f ax1,ax2;
// test
MathTools::eigen4f2axisangle(axisRot1,ax1,ang1);
@@ -802,7 +802,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
generic6Dof->setOverrideNumSolverIterations(100);
- //float totalMass = 1.f/btBody1->getInvMass() + 1.f/btBody2->getInvMass();
+ //double totalMass = 1.f/btBody1->getInvMass() + 1.f/btBody2->getInvMass();
//generic6Dof->setBreakingImpulseThreshold(2*totalMass);//needed? copied from voronoi demo
@@ -1084,7 +1084,7 @@ void BulletRobot::actuateJoints(btScalar dt)
case ePositionVelocity:
{
btScalar pos = btScalar(getJointAngle(it->first));
- float gain = 0.5;
+ double gain = 0.5;
m->m_targetVelocity = it->second.jointVelocityTarget + gain*(it->second.jointValueTarget - pos) / dt;
break;
}
@@ -1153,7 +1153,7 @@ bool BulletRobot::hasLink( VirtualRobot::RobotNodePtr node )
return false;
}
-void BulletRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointValue )
+void BulletRobot::actuateNode( VirtualRobot::RobotNodePtr node, double jointValue )
{
VR_ASSERT(node);
@@ -1206,7 +1206,7 @@ void BulletRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointValue
}
}
-void BulletRobot::actuateNodeVel(RobotNodePtr node, float jointVelocity)
+void BulletRobot::actuateNodeVel(RobotNodePtr node, double jointVelocity)
{
VR_ASSERT(node);
@@ -1259,7 +1259,7 @@ void BulletRobot::actuateNodeVel(RobotNodePtr node, float jointVelocity)
}
}
-float BulletRobot::getJointAngle( VirtualRobot::RobotNodePtr rn )
+double BulletRobot::getJointAngle( VirtualRobot::RobotNodePtr rn )
{
VR_ASSERT(rn);
if (!hasLink(rn))
@@ -1274,8 +1274,8 @@ float BulletRobot::getJointAngle( VirtualRobot::RobotNodePtr rn )
btRotationalLimitMotor *m = dof->getRotationalLimitMotor(0);
VR_ASSERT(m);
dof->calculateTransforms();
- float a1 = dof->getAngle(0);
- float a2 = m->m_currentPosition;
+ double a1 = dof->getAngle(0);
+ double a2 = m->m_currentPosition;
if (fabs(a1-a2)>0.05f)
{
VR_INFO << "Angle diff " << a1 << ", " << a2 << endl;
@@ -1300,14 +1300,14 @@ float BulletRobot::getJointAngle( VirtualRobot::RobotNodePtr rn )
return 0.0f;
VR_ASSERT(m);
hinge2->calculateTransforms();
- float a2 = m->m_currentPosition;
+ double a2 = m->m_currentPosition;
return (a2-link.jointValueOffset);// inverted joint direction in bullet
}
return (hinge->getHingeAngle()-link.jointValueOffset);// inverted joint direction in bullet
#endif
}
-float BulletRobot::getJointTargetSpeed( VirtualRobot::RobotNodePtr rn )
+double BulletRobot::getJointTargetSpeed( VirtualRobot::RobotNodePtr rn )
{
VR_ASSERT(rn);
if (!hasLink(rn))
@@ -1338,7 +1338,7 @@ float BulletRobot::getJointTargetSpeed( VirtualRobot::RobotNodePtr rn )
return hinge->getMotorTargetVelosity();
}
-float BulletRobot::getJointSpeed( VirtualRobot::RobotNodePtr rn )
+double BulletRobot::getJointSpeed( VirtualRobot::RobotNodePtr rn )
{
VR_ASSERT(rn);
if (!hasLink(rn))
@@ -1360,12 +1360,12 @@ float BulletRobot::getJointSpeed( VirtualRobot::RobotNodePtr rn )
Eigen::Vector3f deltaVel = link.dynNode1->getAngularVelocity() - link.dynNode2->getAngularVelocity();
boost::shared_ptr<RobotNodeRevolute> rnRevJoint = boost::dynamic_pointer_cast<RobotNodeRevolute>(link.nodeJoint);
- float speed = deltaVel.dot(rnRevJoint->getJointRotationAxis());
+ double speed = deltaVel.dot(rnRevJoint->getJointRotationAxis());
return speed;//hinge->getMotorTargetVelosity();
#endif
}
-float BulletRobot::getNodeTarget( VirtualRobot::RobotNodePtr node )
+double BulletRobot::getNodeTarget( VirtualRobot::RobotNodePtr node )
{
#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
return DynamicsRobot::getNodeTarget(node);
@@ -1412,7 +1412,7 @@ Eigen::Matrix4f BulletRobot::getComGlobal( VirtualRobot::RobotNodePtr rn )
Eigen::Vector3f BulletRobot::getComGlobal( VirtualRobot::RobotNodeSetPtr set)
{
Eigen::Vector3f com = Eigen::Vector3f::Zero();
- float totalMass = 0.0;
+ double totalMass = 0.0;
for (int i = 0; i < set->getSize(); i++)
{
VirtualRobot::RobotNodePtr node = (*set)[i];
@@ -1429,7 +1429,7 @@ Eigen::Vector3f BulletRobot::getComGlobal( VirtualRobot::RobotNodeSetPtr set)
Eigen::Vector3f BulletRobot::getComGlobalVelocity( VirtualRobot::RobotNodeSetPtr set)
{
Eigen::Vector3f com = Eigen::Vector3f::Zero();
- float totalMass = 0.0;
+ double totalMass = 0.0;
for (int i = 0; i < set->getSize(); i++)
{
VirtualRobot::RobotNodePtr node = (*set)[i];
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
index 6b6fc81f5c1075a60d805c2a85ee0409f547ab9b..e26bc044cf779222ee0a46d993b1963610bd49af 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
@@ -55,7 +55,7 @@ public:
BulletObjectPtr dynNode2; // child
std::vector< std::pair<DynamicsObjectPtr,DynamicsObjectPtr> > disabledCollisionPairs;
boost::shared_ptr<btTypedConstraint> joint;
- float jointValueOffset; // offset simox -> bullet joint values
+ double jointValueOffset; // offset simox -> bullet joint values
};
@@ -98,8 +98,8 @@ public:
std::vector<LinkInfo> getLinks();
- virtual void actuateNode(VirtualRobot::RobotNodePtr node, float jointValue);
- virtual void actuateNodeVel(VirtualRobot::RobotNodePtr node, float jointVelocity);
+ virtual void actuateNode(VirtualRobot::RobotNodePtr node, double jointValue);
+ virtual void actuateNodeVel(VirtualRobot::RobotNodePtr node, double jointVelocity);
/*!
Usually this method is called by the framework in every tick to perform joint actuation.
@@ -109,10 +109,10 @@ public:
virtual void updateSensors();
- virtual float getJointAngle(VirtualRobot::RobotNodePtr rn);
- virtual float getJointSpeed(VirtualRobot::RobotNodePtr rn);
- virtual float getJointTargetSpeed(VirtualRobot::RobotNodePtr rn);
- virtual float getNodeTarget(VirtualRobot::RobotNodePtr node);
+ virtual double getJointAngle(VirtualRobot::RobotNodePtr rn);
+ virtual double getJointSpeed(VirtualRobot::RobotNodePtr rn);
+ virtual double getJointTargetSpeed(VirtualRobot::RobotNodePtr rn);
+ virtual double getNodeTarget(VirtualRobot::RobotNodePtr node);
/*!
* \brief getJointTorques retrieves the torques in the given joint.
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobotLogger.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobotLogger.h
index c247be26af5de0a7c51108e4d6b1575062e2599c..1e9adeeac6e4bff52fa91099d2d932ea15534168 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobotLogger.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobotLogger.h
@@ -43,8 +43,8 @@ private:
std::vector<Eigen::VectorXf> actualVelocityLog;
std::vector<Eigen::Vector3f> actualCoMLog;
std::vector<Eigen::Vector3f> actualCoMVelocityLog;
- std::vector<float> timestamps;
- float timestamp;
+ std::vector<double> timestamps;
+ double timestamp;
bool running;
int max_samples;
diff --git a/SimDynamics/DynamicsEngine/DynamicsEngine.cpp b/SimDynamics/DynamicsEngine/DynamicsEngine.cpp
index 06ad319f9a1aa880b94d6420f5d320ede8eae7f0..006f23eb14b0805d6f2f9218567a2633604f5006 100644
--- a/SimDynamics/DynamicsEngine/DynamicsEngine.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsEngine.cpp
@@ -148,7 +148,7 @@ bool DynamicsEngine::checkCollisionEnabled( DynamicsObject* o1 )
return (i1 == collisionToAllDisabled.end());
}
-void DynamicsEngine::getFloorInfo(Eigen::Vector3f &floorPos, Eigen::Vector3f &floorUp, float &floorExtendMM, float &floorDepthMM)
+void DynamicsEngine::getFloorInfo(Eigen::Vector3f &floorPos, Eigen::Vector3f &floorUp, double &floorExtendMM, double &floorDepthMM)
{
floorPos = this->floorPos;
floorUp = this->floorUp;
diff --git a/SimDynamics/DynamicsEngine/DynamicsEngine.h b/SimDynamics/DynamicsEngine/DynamicsEngine.h
index b07f8a08f41cd24e48be273f88b74ebc9efda8b8..35f38de3b8f62891a692a8393401852fc2c27a6a 100644
--- a/SimDynamics/DynamicsEngine/DynamicsEngine.h
+++ b/SimDynamics/DynamicsEngine/DynamicsEngine.h
@@ -127,7 +127,7 @@ public:
/*!
Stores current floor description to floorPos and floorUp.
*/
- void getFloorInfo(Eigen::Vector3f &floorPos, Eigen::Vector3f &floorUp, float &floorExtendMM, float &floorDepthMM);
+ void getFloorInfo(Eigen::Vector3f &floorPos, Eigen::Vector3f &floorUp, double &floorExtendMM, double &floorDepthMM);
struct DynamicsContactInfo
{
@@ -136,9 +136,9 @@ public:
Eigen::Vector3f posGlobalA;
Eigen::Vector3f posGlobalB;
Eigen::Vector3f normalGlobalB;
- float combinedFriction;
- float combinedRestitution;
- float appliedImpulse;
+ double combinedFriction;
+ double combinedRestitution;
+ double appliedImpulse;
};
virtual std::vector<DynamicsEngine::DynamicsContactInfo> getContacts();
@@ -162,8 +162,8 @@ protected:
Eigen::Vector3f floorPos;
Eigen::Vector3f floorUp;
- float floorExtendMM;
- float floorDepthMM;
+ double floorExtendMM;
+ double floorDepthMM;
boost::recursive_mutex engineMutex;
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
index e106522f63c5a6b0e9d0ef2caa966c0aa6d0e5d7..4139adfaeaae4fd438bc627235c3c4f269f7b8c8 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
@@ -69,14 +69,14 @@ DynamicsObjectPtr DynamicsRobot::getDynamicsRobotNode( VirtualRobot::RobotNodePt
return dynamicRobotNodes[node];
}
-void DynamicsRobot::actuateNode(const std::string &node, float jointValue )
+void DynamicsRobot::actuateNode(const std::string &node, double jointValue )
{
VR_ASSERT(robot);
VR_ASSERT(robot->hasRobotNode(node));
actuateNode(robot->getRobotNode(node),jointValue);
}
-void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointValue )
+void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, double jointValue )
{
VR_ASSERT(robot);
VR_ASSERT(node);
@@ -105,7 +105,7 @@ void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointVal
}
}
-void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointValue , float jointVelocity)
+void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, double jointValue , double jointVelocity)
{
VR_ASSERT(robot);
VR_ASSERT(node);
@@ -129,7 +129,7 @@ void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointVal
}
}
-void DynamicsRobot::actuateNodeVel(const std::string &node, float jointVelocity )
+void DynamicsRobot::actuateNodeVel(const std::string &node, double jointVelocity )
{
VR_ASSERT(robot);
VR_ASSERT(robot->hasRobotNode(node));
@@ -137,7 +137,7 @@ void DynamicsRobot::actuateNodeVel(const std::string &node, float jointVelocity
actuateNodeVel(robot->getRobotNode(node), jointVelocity);
}
-void DynamicsRobot::actuateNodeVel( VirtualRobot::RobotNodePtr node, float jointVelocity )
+void DynamicsRobot::actuateNodeVel( VirtualRobot::RobotNodePtr node, double jointVelocity )
{
VR_ASSERT(robot);
VR_ASSERT(node);
@@ -164,14 +164,14 @@ void DynamicsRobot::actuateNodeVel( VirtualRobot::RobotNodePtr node, float joint
}
}
-void DynamicsRobot::actuateNodeTorque(const std::string &node, float jointTorque )
+void DynamicsRobot::actuateNodeTorque(const std::string &node, double jointTorque )
{
VR_ASSERT(robot);
VR_ASSERT(robot->hasRobotNode(node));
actuateNodeTorque(robot->getRobotNode(node),jointTorque);
}
-void DynamicsRobot::actuateNodeTorque( VirtualRobot::RobotNodePtr node, float jointTorque )
+void DynamicsRobot::actuateNodeTorque( VirtualRobot::RobotNodePtr node, double jointTorque )
{
VR_ASSERT(robot);
VR_ASSERT(node);
@@ -224,7 +224,7 @@ void DynamicsRobot::disableActuation()
it++;
}
}
-void DynamicsRobot::actuateJoints(float dt)
+void DynamicsRobot::actuateJoints(double dt)
{
}
@@ -237,7 +237,7 @@ bool DynamicsRobot::isNodeActuated( VirtualRobot::RobotNodePtr node )
return actuationTargets[node].actuation.mode != 0;
}
-float DynamicsRobot::getNodeTarget( VirtualRobot::RobotNodePtr node )
+double DynamicsRobot::getNodeTarget( VirtualRobot::RobotNodePtr node )
{
VR_ASSERT(node);
if (actuationTargets.find(node) == actuationTargets.end())
@@ -246,17 +246,17 @@ float DynamicsRobot::getNodeTarget( VirtualRobot::RobotNodePtr node )
}
-float DynamicsRobot::getJointAngle( VirtualRobot::RobotNodePtr rn )
+double DynamicsRobot::getJointAngle( VirtualRobot::RobotNodePtr rn )
{
return 0.0f;
}
-float DynamicsRobot::getJointSpeed( VirtualRobot::RobotNodePtr rn )
+double DynamicsRobot::getJointSpeed( VirtualRobot::RobotNodePtr rn )
{
return 0.0f;
}
-float DynamicsRobot::getJointTargetSpeed( VirtualRobot::RobotNodePtr rn )
+double DynamicsRobot::getJointTargetSpeed( VirtualRobot::RobotNodePtr rn )
{
return 0.0f;
}
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.h b/SimDynamics/DynamicsEngine/DynamicsRobot.h
index 0f93b39c87555644f2a7b505abc2301a6e5539d8..da2fb9125cf2f0cce7a55477c8c3e55185a61331 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.h
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.h
@@ -62,16 +62,16 @@ public:
/*!
Enable joint actuation for given node.
*/
- virtual void actuateNode(VirtualRobot::RobotNodePtr node, float jointValue, float jointVelocity);
- virtual void actuateNode(VirtualRobot::RobotNodePtr node, float jointValue);
- virtual void actuateNodeVel(VirtualRobot::RobotNodePtr node, float jointVelocity);
- virtual void actuateNodeTorque(VirtualRobot::RobotNodePtr node, float jointTorque);
- virtual void actuateNode(const std::string &node, float jointValue);
- virtual void actuateNodeVel(const std::string &node, float jointVelocity);
- virtual void actuateNodeTorque(const std::string &node, float jointTorque);
+ virtual void actuateNode(VirtualRobot::RobotNodePtr node, double jointValue, double jointVelocity);
+ virtual void actuateNode(VirtualRobot::RobotNodePtr node, double jointValue);
+ virtual void actuateNodeVel(VirtualRobot::RobotNodePtr node, double jointVelocity);
+ virtual void actuateNodeTorque(VirtualRobot::RobotNodePtr node, double jointTorque);
+ virtual void actuateNode(const std::string &node, double jointValue);
+ virtual void actuateNodeVel(const std::string &node, double jointVelocity);
+ virtual void actuateNodeTorque(const std::string &node, double jointTorque);
virtual void disableNodeActuation(VirtualRobot::RobotNodePtr node);
virtual bool isNodeActuated(VirtualRobot::RobotNodePtr node);
- virtual float getNodeTarget(VirtualRobot::RobotNodePtr node);
+ virtual double getNodeTarget(VirtualRobot::RobotNodePtr node);
virtual void enableActuation(ActuationMode mode);
virtual void disableActuation();
@@ -79,7 +79,7 @@ public:
Usually this method is called by the framework in every tick to perform joint actuation.
\param dt Timestep
*/
- virtual void actuateJoints(float dt);
+ virtual void actuateJoints(double dt);
virtual void updateSensors(){}
// experimental...
@@ -92,9 +92,9 @@ public:
//virtual void setPose(const Eigen::Matrix4f &pose);
- virtual float getJointAngle(VirtualRobot::RobotNodePtr rn);
- virtual float getJointSpeed(VirtualRobot::RobotNodePtr rn);
- virtual float getJointTargetSpeed(VirtualRobot::RobotNodePtr rn);
+ virtual double getJointAngle(VirtualRobot::RobotNodePtr rn);
+ virtual double getJointSpeed(VirtualRobot::RobotNodePtr rn);
+ virtual double getJointTargetSpeed(VirtualRobot::RobotNodePtr rn);
virtual Eigen::Matrix4f getComGlobal(VirtualRobot::RobotNodePtr rn);
@@ -114,9 +114,9 @@ protected:
{
actuation.mode = 0;
}
- float jointValueTarget;
- float jointVelocityTarget;
- float jointTorqueTarget;
+ double jointValueTarget;
+ double jointVelocityTarget;
+ double jointTorqueTarget;
VirtualRobot::RobotNodePtr node;
//DynamicsObjectPtr dynNode; // if node is a joint without model, there is no dyn node!
ActuationMode actuation;
diff --git a/SimDynamics/DynamicsEngine/DynamicsUtils.cpp b/SimDynamics/DynamicsEngine/DynamicsUtils.cpp
index 6ef55800ff8e10856eb2f1a68958d478c3235d6e..564f780d21a9ab143351244858e52f4bdc41d513 100644
--- a/SimDynamics/DynamicsEngine/DynamicsUtils.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsUtils.cpp
@@ -4,7 +4,7 @@
#include <iostream>
namespace SimDynamics {
-PIDController::PIDController(float gainP, float gainI, float gainD)
+PIDController::PIDController(double gainP, double gainI, double gainD)
: gainP(gainP)
, gainI(gainI)
, gainD(gainD)
@@ -13,15 +13,15 @@ PIDController::PIDController(float gainP, float gainI, float gainD)
{
}
-float PIDController::update(float error, float dt)
+double PIDController::update(double error, double dt)
{
- float p = error * gainP;
+ double p = error * gainP;
errorSum += error * dt;
- float i = errorSum * gainI;
- float d = (error - lastError)/dt * gainD;
+ double i = errorSum * gainI;
+ double d = (error - lastError)/dt * gainD;
lastError = error;
- float output = (p + i + d);
+ double output = (p + i + d);
lastOutput = output;
return output;
}
@@ -57,11 +57,11 @@ TorqueMotorController::TorqueMotorController(const PIDController& positionContro
}
-float TorqueMotorController::update(float positionError, float velocityError, float torqueError, ActuationMode actuation, float dt)
+double TorqueMotorController::update(double positionError, double velocityError, double torqueError, ActuationMode actuation, double dt)
{
- float posUpdate = 0.0;
- float velUpdate = 0.0;
- float torqueUpdate = 0.0;
+ double posUpdate = 0.0;
+ double velUpdate = 0.0;
+ double torqueUpdate = 0.0;
if (actuation.modes.position)
posUpdate = positionController.update(positionError, dt);
else
@@ -92,7 +92,7 @@ float TorqueMotorController::update(float positionError, float velocityError, fl
}
VelocityMotorController::VelocityMotorController()
-: positionController(100.0, 50.0, 0.0)
+: positionController(100.0, 10.0, 0.0)
{
}
@@ -102,15 +102,15 @@ VelocityMotorController::VelocityMotorController(const PIDController& positionCo
}
-float VelocityMotorController::update(float positionError, float targetVelocity, ActuationMode actuation, float dt)
+double VelocityMotorController::update(double positionError, double targetVelocity, ActuationMode actuation, double dt)
{
- float posUpdate = 0.0;
+ double posUpdate = 0.0;
if (actuation.modes.position)
posUpdate = positionController.update(positionError, dt);
- else
- positionController.reset();
+ //else
+ // positionController.reset();
- float output = 0.0f;
+ double output = 0.0f;
if (actuation.modes.position && actuation.modes.velocity)
output = posUpdate + targetVelocity;
else if (actuation.modes.position)
diff --git a/SimDynamics/DynamicsEngine/DynamicsUtils.h b/SimDynamics/DynamicsEngine/DynamicsUtils.h
index d5d3d5686d6de408ff62364cdd22ee6454f9f899..52620d2dde5ab278fa6000e515974f2c671c25c1 100644
--- a/SimDynamics/DynamicsEngine/DynamicsUtils.h
+++ b/SimDynamics/DynamicsEngine/DynamicsUtils.h
@@ -6,21 +6,21 @@
namespace SimDynamics {
class PIDController {
public:
- PIDController(float gainP, float gainI, float gainD);
+ PIDController(double gainP, double gainI, double gainD);
- float update(float error, float dt);
+ double update(double error, double dt);
void reset();
void debug();
private:
- float gainP;
- float gainI;
- float gainD;
- float errorSum;
- float lastError;
- float lastOutput;
+ double gainP;
+ double gainI;
+ double gainD;
+ double errorSum;
+ double lastError;
+ double lastOutput;
};
// use bit field because enums are a pain
@@ -65,7 +65,7 @@ public:
const PIDController& velocityController,
const PIDController& torqueController);
- float update(float positionError, float velocityError, float torqueError, ActuationMode actuation, float dt);
+ double update(double positionError, double velocityError, double torqueError, ActuationMode actuation, double dt);
private:
PIDController positionController;
@@ -99,7 +99,7 @@ public:
VelocityMotorController(const PIDController& positionController);
- float update(float positionError, float targetVelocity, ActuationMode actuation, float dt);
+ double update(double positionError, double targetVelocity, ActuationMode actuation, double dt);
void reset();