diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
index 603cd8468f2dddecdbaad989f2fe3b75c897e38c..ca7ea1878898a611d009a9c3c51d92bc38cfc104 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
@@ -395,7 +395,7 @@ void BulletEngine::updateRobots(btScalar timeStep)
{
for (size_t i=0; i < robots.size();i++)
{
- robots[i]->actuateJoints(timeStep);
+ robots[i]->actuateJoints(double(timeStep));
robots[i]->updateSensors();
}
}
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
index 96e2eb3d8fc0fd208aaa9822b7a6e3ed8d7be520..e30ae00192c1f752888e921f9ddadfbfe268182d 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
@@ -52,9 +52,9 @@ namespace SimDynamics
bulletSolverIterations = 100;
bulletSolverGlobalContactForceMixing = 0;
bulletSolverGlobalErrorReductionParameter = btScalar(0.2);//0.1);
- bulletSolverSuccessiveOverRelaxation = 1.3;
- bulletSolverContactSurfaceLayer = 0.001;
- bulletSolverSplitImpulsePenetrationThreshold = -0.01;
+ bulletSolverSuccessiveOverRelaxation = btScalar(1.3);
+ bulletSolverContactSurfaceLayer = btScalar(0.001);
+ bulletSolverSplitImpulsePenetrationThreshold = btScalar(-0.01);
}
virtual ~BulletEngineConfig(){}
@@ -119,7 +119,6 @@ public:
*/
virtual void createFloorPlane(const Eigen::Vector3f &pos, const Eigen::Vector3f &up);
-
virtual void stepSimulation(double dt, int maxSubSteps, double fixedTimeStep);
btDynamicsWorld* getBulletWorld();
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index fba48342c213f3f1185ef6d88bc2055e9f109c61..df3fb7eafae6f4e2366f1b66a7056fbbd4d10ddd 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -30,7 +30,7 @@ BulletRobot::BulletRobot(VirtualRobot::RobotPtr rob, bool enableJointMotors)
{
bulletMaxMotorImulse = 1500.0f;
- bulletMotorVelFactor = 10.0f;
+ bulletMotorVelFactor = 100.0f;
buildBulletModels(enableJointMotors);
// activate force torque sensors
@@ -133,171 +133,16 @@ void BulletRobot::buildBulletModels(bool enableJointMotors)
}
if (!bodyA)
{
- // root node
- /*if (joint)
- {
- THROW_VR_EXCEPTION ("Not able to build valid dynamic model: First node of robot with collision model (" << rn->getName() << ") is connected with a joint (" << joint->getName() <<") but no parent collision model found.");
- }*/
bodyA = robot->getRootNode();
}
- //} else
- {
- /* Eigen::Matrix4f trafoA2J = Eigen::Matrix4f::Identity(); // bodyA->joint
- Eigen::Matrix4f trafoJ2B = Eigen::Matrix4f::Identity(); // joint->bodyB
- if (joint)
- {
- // now we have bodyA->joint->bodyB
- // compute trafoA2J
- std::vector<Eigen::Matrix4f> trafosA;
- // visualization is affected by preJointTransformation
- trafosA.push_back(joint->getPreJointTransformation());
-
- // go parents up until bodyA is reached
- parent = boost::dynamic_pointer_cast<RobotNode>(joint->getParent());
- while (parent && parent!=bodyA)
- {
- trafosA.push_back(parent->getPostJointTransformation());
- trafosA.push_back(parent->getPreJointTransformation());
- parent = boost::dynamic_pointer_cast<RobotNode>(parent->getParent());
- }
- THROW_VR_EXCEPTION_IF(!parent,"internal error, no parent 2");
- trafosA.push_back(bodyA->getPostJointTransformation());
- std::vector<Eigen::Matrix4f>::reverse_iterator rit;
- // accumulate transformations from bodyA_post to joint_pre
- for (rit=trafosA.rbegin(); rit != trafosA.rend(); rit++)
- {
- trafoA2J *= *rit;
- }
- // compute trafoJ2B
- std::vector<Eigen::Matrix4f> trafosB;
-
- // visualization is affected by preJointTransformation
- trafosB.push_back(bodyB->getPreJointTransformation());
- if (joint!=bodyB)
- {
- // go parents up until joint is reached
- parent = boost::dynamic_pointer_cast<RobotNode>(bodyB->getParent());
- while (parent && parent!=joint)
- {
- trafosB.push_back(parent->getPostJointTransformation());
- trafosB.push_back(parent->getPreJointTransformation());
- parent = boost::dynamic_pointer_cast<RobotNode>(parent->getParent());
- }
- THROW_VR_EXCEPTION_IF(!parent,"internal error, no parent");
- trafosB.push_back(joint->getPostJointTransformation());
- }
- // accumulate transformations from joint_post to bodyB_pre
- for (rit=trafosB.rbegin(); rit != trafosB.rend(); rit++)
- {
- trafoJ2B *= *rit;
- }
- } else
- {
- // fixed joint: bodyA -> bodyB
- joint = bodyB;
-
- // compute trafoA2J (which is trafo from A to B)
- std::vector<Eigen::Matrix4f> trafosA;
- // visualization is affected by preJointTransformation
- trafosA.push_back(bodyB->getPreJointTransformation());
-
- // go parents up until bodyA is reached
- parent = boost::dynamic_pointer_cast<RobotNode>(bodyB->getParent());
- while (parent && parent!=bodyA)
- {
- trafosA.push_back(parent->getPostJointTransformation());
- trafosA.push_back(parent->getPreJointTransformation());
- parent = boost::dynamic_pointer_cast<RobotNode>(parent->getParent());
- }
- THROW_VR_EXCEPTION_IF(!parent,"internal error, no parent 3");
- trafosA.push_back(bodyA->getPostJointTransformation());
- std::vector<Eigen::Matrix4f>::reverse_iterator rit;
- // accumulate transformations from bodyA_post to joint_pre
- for (rit=trafosA.rbegin(); rit != trafosA.rend(); rit++)
- {
- trafoA2J *= *rit;
- }
-
- }*/
+ // check for fixed joint
+ if (!joint)
+ joint = bodyB;
- // check for fixed joint
- if (!joint)
- joint = bodyB;
-
- createLink(bodyA,joint,joint2,bodyB);//,trafoA2J,trafoJ2B);
- }
-
- }
-
- /*
- RobotNodePtr bodyA;
- RobotNodePtr bodyB;
- RobotNodePtr joint;
- Eigen::Matrix4f trafoA = Eigen::Matrix4f;
- Eigen::Matrix4f trafoB = Eigen::Matrix4f;
- for (size_t i=0;i<robotNodes.size();i++)
- {
-
- RobotNodePtr rn = robotNodes[i];
-
- switch(rn->getType())
- {
- case RobotNode::Generic:
- THROW_VR_EXCEPTION ("Could not build dynamic model. Only <JointNode> <TransformationNode> and <BodyNode> tags allowed!");
- break;
- case RobotNode::Body:
- break;
- default:
- continue;
- }
- // a body
- bodyB = rn;
- bodyA.reset();
- joint.reset();
- trafoA = Eigen::Matrix4f;
- trafoB = Eigen::Matrix4f;
-
- // search parent body
- RobotNodePtr parent = rn->getParent();
- bool built = false;
- while (parent && !built)
- {
- switch (parent->getType())
- {
- case RobotNode::Joint:
- if (joint)
- {
- THROW_VR_EXCEPTION("Need bodies between joints. Two succeeding joints found in kinematic structure");
- }
- joint = parent;
- break;
- case RobotNode::Body:
- if (!joint)
- {
- THROW_VR_EXCEPTION("Need joint between body. Two succeeding bodies found in kinematic structure");
- }
- createLink(bodyA,joint,bodyB,trafoA,trafoB);
- built = true;
- break;
- case RobotNode::Transform:
- if (!joint)
- trafoA *= parent->getPostJointTransformation
- built = true;
- break;
- }
+ createLink(bodyA,joint,joint2,bodyB);//,trafoA2J,trafoJ2B);
}
- */
-
- /*
- std::vector<SceneObjectPtr> children = rn->getChildren();
- for (size_t c=0;c<children.size();c++)
- {
- RobotNodePtr rn2 = boost::dynamic_pointer_cast<RobotNode>(children[c]);
- if (rn2)
- createLink(rn,rn2,enableJointMotors);
- }*/
}
}
@@ -892,7 +737,7 @@ void BulletRobot::ensureKinematicConstraints()
#endif
}
-void BulletRobot::actuateJoints(btScalar dt)
+void BulletRobot::actuateJoints(double dt)
{
//cout << "=== === BulletRobot: actuateJoints() 1 === " << endl;
@@ -917,7 +762,7 @@ void BulletRobot::actuateJoints(btScalar dt)
btScalar posTarget = btScalar(it->second.jointValueTarget + link.jointValueOffset);
btScalar posActual = btScalar(getJointAngle(it->first));
- btScalar velocityTarget = it->second.jointVelocityTarget;
+ btScalar velocityTarget = btScalar(it->second.jointVelocityTarget);
#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
boost::shared_ptr<btGeneric6DofConstraint> dof = boost::dynamic_pointer_cast<btGeneric6DofConstraint>(link.joint);
@@ -957,20 +802,20 @@ void BulletRobot::actuateJoints(btScalar dt)
if (actuation.modes.position && actuation.modes.velocity)
{
hinge->enableAngularMotor(true,
- controller.update(posTarget - posActual, velocityTarget, actuation, dt),
+ controller.update(posTarget - posActual, velocityTarget, actuation, btScalar(dt)),
bulletMaxMotorImulse);
}
else if (actuation.modes.position)
{
hinge->enableAngularMotor(true,
- controller.update(posTarget - posActual, 0.0, actuation, dt),
+ controller.update(posTarget - posActual, 0.0, actuation, btScalar(dt)),
bulletMaxMotorImulse);
}
else if (actuation.modes.velocity)
{
hinge->enableAngularMotor(true,
- controller.update(0.0, velocityTarget, actuation, dt),
+ controller.update(0.0, velocityTarget, actuation, btScalar(dt)),
bulletMaxMotorImulse);
}
// FIXME this bypasses the controller (and doesn't work..)
@@ -1407,7 +1252,7 @@ Eigen::Vector3f BulletRobot::getComGlobal( VirtualRobot::RobotNodeSetPtr set)
{
Eigen::Vector3f com = Eigen::Vector3f::Zero();
double totalMass = 0.0;
- for (int i = 0; i < set->getSize(); i++)
+ for (unsigned int i = 0; i < set->getSize(); i++)
{
VirtualRobot::RobotNodePtr node = (*set)[i];
BulletObjectPtr bo = boost::dynamic_pointer_cast<BulletObject>(getDynamicsRobotNode(node));
@@ -1416,7 +1261,7 @@ Eigen::Vector3f BulletRobot::getComGlobal( VirtualRobot::RobotNodeSetPtr set)
totalMass += node->getMass();
}
- com *= 1.0f/totalMass;
+ com *= float(1.0f/totalMass);
return com;
}
@@ -1424,7 +1269,7 @@ Eigen::Vector3f BulletRobot::getComGlobalVelocity( VirtualRobot::RobotNodeSetPtr
{
Eigen::Vector3f com = Eigen::Vector3f::Zero();
double totalMass = 0.0;
- for (int i = 0; i < set->getSize(); i++)
+ for (unsigned int i = 0; i < set->getSize(); i++)
{
VirtualRobot::RobotNodePtr node = (*set)[i];
BulletObjectPtr bo = boost::dynamic_pointer_cast<BulletObject>(getDynamicsRobotNode(node));
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
index e26bc044cf779222ee0a46d993b1963610bd49af..45c441bc4395cf27c4d0711fa56c7c9348d5535b 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
@@ -105,7 +105,7 @@ public:
Usually this method is called by the framework in every tick to perform joint actuation.
\param dt Timestep
*/
- virtual void actuateJoints(btScalar dt);
+ virtual void actuateJoints(double dt);
virtual void updateSensors();
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobotLogger.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobotLogger.cpp
index c14edeef31617d77489a49aa3a5d433ec71f42b7..28e195e030c1f09a3c6b23206eb4d267db11b86d 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobotLogger.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobotLogger.cpp
@@ -73,7 +73,7 @@ void BulletRobotLogger::log(btScalar dt)
if (!running)
return;
- if (actualAngleLog.size() > max_samples)
+ if (int(actualAngleLog.size()) > max_samples)
{
std::cout << "Warning: Exceeded max_samples! Stopping logging." << std::endl;
running = false;
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
index 8f83f961151586119fde3dd8145e976c1e5b4076..aa0d92a8f47990081474b0198140b950016c9914 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
@@ -122,7 +122,7 @@ void SimoxMotionState::setGlobalPoseSimox( const Eigen::Matrix4f& worldPose )
{
if (links[i].nodeJoint)
{
- float ja = bdr->getJointAngle(links[i].nodeJoint);
+ float ja = float(bdr->getJointAngle(links[i].nodeJoint));
// we can update the joint value via an RobotNodeActuator
RobotNodeActuatorPtr rna (new RobotNodeActuator(links[i].nodeJoint));
rna->updateJointAngle(ja);
diff --git a/VirtualRobot/CollisionDetection/CollisionChecker.h b/VirtualRobot/CollisionDetection/CollisionChecker.h
index 5a39820908cf75ae462dfe90a20a9e28fa811894..c5a1e2d8bb35db4c01d4f91f1cab15dd9caecc25 100644
--- a/VirtualRobot/CollisionDetection/CollisionChecker.h
+++ b/VirtualRobot/CollisionDetection/CollisionChecker.h
@@ -63,9 +63,9 @@ public:
virtual float calculateDistance (SceneObjectSetPtr model1, SceneObjectSetPtr model2);
virtual float calculateDistance (CollisionModelPtr model1, SceneObjectSetPtr model2);
virtual float calculateDistance (CollisionModelPtr model1, CollisionModelPtr model2);
- virtual float calculateDistance (SceneObjectSetPtr model1, SceneObjectSetPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1, int* trID2);
- virtual float calculateDistance (CollisionModelPtr model1, SceneObjectSetPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1, int* trID2);
- virtual float calculateDistance (CollisionModelPtr model1, CollisionModelPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1, int* trID2);
+ virtual float calculateDistance(SceneObjectSetPtr model1, SceneObjectSetPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1 = NULL, int* trID2 = NULL);
+ virtual float calculateDistance(CollisionModelPtr model1, SceneObjectSetPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1 = NULL, int* trID2 = NULL);
+ virtual float calculateDistance(CollisionModelPtr model1, CollisionModelPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1 = NULL, int* trID2 = NULL);
/*!
Test if the two models are colliding.
@@ -124,8 +124,8 @@ public:
*/
virtual float calculateDistance (SceneObjectPtr model1, SceneObjectSetPtr model2);
virtual float calculateDistance (SceneObjectPtr model1, SceneObjectPtr model2);
- virtual float calculateDistance (SceneObjectPtr model1, SceneObjectSetPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1, int* trID2);
- virtual float calculateDistance (SceneObjectPtr model1, SceneObjectPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1, int* trID2);
+ virtual float calculateDistance(SceneObjectPtr model1, SceneObjectSetPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1 = NULL, int* trID2 = NULL);
+ virtual float calculateDistance(SceneObjectPtr model1, SceneObjectPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1 = NULL, int* trID2 = NULL);
/*!
Test if the two models are colliding.
diff --git a/VirtualRobot/CollisionDetection/CollisionCheckerImplementation.h b/VirtualRobot/CollisionDetection/CollisionCheckerImplementation.h
index 318794c6e99a97d1e02114fae533fdc6c1398742..7caf88ea48ebe03a14dd5194e37ad8f9861d86f6 100644
--- a/VirtualRobot/CollisionDetection/CollisionCheckerImplementation.h
+++ b/VirtualRobot/CollisionDetection/CollisionCheckerImplementation.h
@@ -41,7 +41,7 @@ public:
CollisionCheckerImplementation(){automaticSizeCheck = true; debugOutput=false;}
virtual ~CollisionCheckerImplementation(){}
- virtual float calculateDistance (CollisionModelPtr model1, CollisionModelPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1, int* trID2) = 0;
+ virtual float calculateDistance(CollisionModelPtr model1, CollisionModelPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1 = NULL, int* trID2 = NULL) = 0;
virtual bool checkCollision (CollisionModelPtr model1, CollisionModelPtr model2) = 0;//, Eigen::Vector3f *storeContact = NULL) = 0;
virtual void setAutomaticSizeCheck(bool checkSizeOnColModelCreation){automaticSizeCheck = checkSizeOnColModelCreation;}
diff --git a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.h b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.h
index 937733eb99d3c57e9cf4db9293b3789b8f3bd111..528e807795b73bc4f921c5a01a63e16e1a6ccbe7 100644
--- a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.h
+++ b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.h
@@ -50,7 +50,7 @@ public:
CollisionCheckerPQP();
virtual ~CollisionCheckerPQP();
- virtual float calculateDistance (CollisionModelPtr model1, CollisionModelPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1, int* trID2);
+ virtual float calculateDistance(CollisionModelPtr model1, CollisionModelPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1 = NULL, int* trID2 = NULL);
virtual bool checkCollision (CollisionModelPtr model1, CollisionModelPtr model2);//, Eigen::Vector3f *storeContact = NULL);
/*!
diff --git a/VirtualRobot/Import/COLLADA-light/ColladaSimox.h b/VirtualRobot/Import/COLLADA-light/ColladaSimox.h
index 5e063001cdf0b606e31e2871b17b18066651af15..11885c6f6fd1196095a9ce60e2413601c8e6802d 100644
--- a/VirtualRobot/Import/COLLADA-light/ColladaSimox.h
+++ b/VirtualRobot/Import/COLLADA-light/ColladaSimox.h
@@ -1,3 +1,6 @@
+#ifndef __COLLADA_SIMOX_H__
+#define __COLLADA_SIMOX_H__
+
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include "inventor.h"
@@ -52,5 +55,7 @@ public:
};
-
} //namespace
+
+
+#endif
diff --git a/VirtualRobot/Import/COLLADA-light/collada.h b/VirtualRobot/Import/COLLADA-light/collada.h
index 8be1d44a248c548e2cafeb053f0c780e5612593c..8b7deb8134dfcba0276cb42b7226ad97304ff5da 100644
--- a/VirtualRobot/Import/COLLADA-light/collada.h
+++ b/VirtualRobot/Import/COLLADA-light/collada.h
@@ -1,5 +1,5 @@
-#ifndef __COLLADA_H__
-#define __COLLADA_H__
+#ifndef __COLLADA_SIMOX_IMPORT_H__
+#define __COLLADA_SIMOX_IMPORT_H__
#include "pugixml/pugixml.hpp"