diff --git a/CMakeLists.txt b/CMakeLists.txt
index 10d2e5c1d6b63b8f943d9f6d4496841f2c04dcd4..b2bb71f72ace504cbb4d7b05ab94a7e847a1b1b8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -33,6 +33,9 @@ add_subdirectory(doc)
MESSAGE (STATUS "\n ***** Simox: Generating CMake files for build and install setup")
set(Simox_LIBRARIES VirtualRobot Saba GraspStudio)
+if (Simox_BUILD_SimDynamics)
+ set(Simox_LIBRARIES "Simox_LIBRARIES" "SimDynamics")
+endif()
set(Simox_EXECUTABLES "")
# Export the package for use from the build-tree
@@ -48,10 +51,22 @@ include ("${VirtualRobot_CONFIG_INBUILD}")
# Create an SimoxConfig.cmake file for the use from the build tree
SET(Simox_LIBRARIES_ALL "${VirtualRobot_LIBRARIES}" "Saba" "GraspStudio")
+if (Simox_BUILD_SimDynamics)
+ SET(Simox_LIBRARIES_ALL "${Simox_LIBRARIES_ALL}" "SimDynamics" "${SimDynamics_EXTERNAL_LIBRARIES}")
+ #MESSAGE ("Simox_LIBRARIES_ALL: ${Simox_LIBRARIES_ALL}")
+ #MESSAGE ("SimDynamics_PHYSICS_LIBRARIES: ${SimDynamics_PHYSICS_LIBRARIES}")
+ #MESSAGE ("SABA_INCLUDE_DIRS: ${SABA_INCLUDE_DIRS}")
+
+endif()
SET(Simox_BASE_DIR "${PROJECT_SOURCE_DIR}")
-SET(Simox_INCLUDE_DIRS "${VirtualRobot_INCLUDE_DIRS}" "${GRASP_STUDIO_INCLUDE_DIRS}" "${SABA_INCLUDE_DIRS}")
-SET(Simox_LIB_DIRS "${VirtualRobot_LIBRARY_DIRS}" "${GRASP_STUDIO_LIBRARY_DIRS}" "${SABA_LIBRARY_DIRS}")
-SET(Simox_LIB_FLAGS "${VirtualRobot_COMPILE_FLAGS} ${GRASP_STUDIO_COMPILE_FLAGS} ${SABA_COMPILE_FLAGS}")
+SET(Simox_INCLUDE_DIRS "${VirtualRobot_INCLUDE_DIRS}" "${GRASP_STUDIO_INCLUDE_DIRS}" "${SABA_INCLUDE_DIRS}" "${SIMDYNAMICS_INCLUDE_DIRS}")
+ #MESSAGE ("VirtualRobot_INCLUDE_DIRS: ${VirtualRobot_INCLUDE_DIRS}")
+ #MESSAGE ("Simox_INCLUDE_DIRS: ${Simox_INCLUDE_DIRS}")
+ #MESSAGE ("Simox_LIBRARIES_ALL: ${Simox_LIBRARIES_ALL}")
+SET(Simox_LIB_DIRS "${VirtualRobot_LIBRARY_DIRS}" "${GRASP_STUDIO_LIBRARY_DIRS}" "${SABA_LIBRARY_DIRS}" "${SIMDYNAMICS_LIBRARY_DIRS}")
+ #MESSAGE ("Simox_LIB_DIRS: ${Simox_LIB_DIRS}")
+SET(Simox_LIB_FLAGS "${VirtualRobot_COMPILE_FLAGS} ${GRASP_STUDIO_COMPILE_FLAGS} ${SABA_COMPILE_FLAGS} ${SIMDYNAMICS_COMPILE_FLAGS}")
+ #MESSAGE ("Simox_LIB_FLAGS: ${Simox_LIB_FLAGS}")
SET(Simox_CMAKE_DIR "${Simox_BUILD_DIRECTORY}/share/Simox/cmake")
SET(VirtualRobot_CMAKE_DIR "${Simox_BUILD_DIRECTORY}/share/VirtualRobot/cmake")
SET(Simox_LIBRARY_CMAKE_INCLUDE "${VirtualRobot_CMAKE_INCLUDE}")
@@ -86,9 +101,9 @@ include ("${VirtualRobot_CONFIG_INSTALL_LOCAL}")
include(${PROJECT_SOURCE_DIR}/VirtualRobot/CMakeModules/VirtualRobotExternalLibrarySetup.cmake)
include(${PROJECT_SOURCE_DIR}/VirtualRobot/CMakeModules/VirtualRobotMacros.cmake)
-SET(Simox_INCLUDE_DIRS "${VirtualRobot_INCLUDE_DIRS}" "${GRASP_STUDIO_INCLUDE_DIRS}" "${SABA_INCLUDE_DIRS}")
-SET(Simox_LIB_DIRS "${VirtualRobot_LIBRARY_DIRS}" "${GRASP_STUDIO_LIBRARY_DIRS}" "${SABA_LIBRARY_DIRS}")
-SET(Simox_LIB_FLAGS "${VirtualRobot_COMPILE_FLAGS} ${GRASP_STUDIO_COMPILE_FLAGS} ${SABA_COMPILE_FLAGS}")
+SET(Simox_INCLUDE_DIRS "${VirtualRobot_INCLUDE_DIRS}" "${GRASP_STUDIO_INCLUDE_DIRS}" "${SABA_INCLUDE_DIRS}" "${SIMDYNAMICS_INCLUDE_DIRS}")
+SET(Simox_LIB_DIRS "${VirtualRobot_LIBRARY_DIRS}" "${GRASP_STUDIO_LIBRARY_DIRS}" "${SABA_LIBRARY_DIRS}" "${SIMDYNAMICS_LIBRARY_DIRS}")
+SET(Simox_LIB_FLAGS "${VirtualRobot_COMPILE_FLAGS} ${GRASP_STUDIO_COMPILE_FLAGS} ${SABA_COMPILE_FLAGS} ${SIMDYNAMICS_COMPILE_FLAGS}")
SET(Simox_BASE_DIR "${CMAKE_INSTALL_PREFIX}/include")
SET(Simox_CMAKE_DIR "${CMAKE_INSTALL_PREFIX}/share/Simox/cmake")
SET(VirtualRobot_CMAKE_DIR "${CMAKE_INSTALL_PREFIX}/share/VirtualRobot/cmake")
diff --git a/SimDynamics/CMakeLists.txt b/SimDynamics/CMakeLists.txt
index fd5158437d8fcfce8b626128d94ea5279ad2559f..c1a023973de836c2b7d810098d27d9adfd094f3c 100644
--- a/SimDynamics/CMakeLists.txt
+++ b/SimDynamics/CMakeLists.txt
@@ -76,8 +76,8 @@ if (SimDynamics_DYNAMICSENGINE)
endif ()
# this var is considered for generating Simox_INCLUDE_DIRS_
- SET(SimDynamics_INCLUDE_DIRS "${SimDynamics_SimoxDir}" "${SimDynamics_DIR}")
-
+ SET(SimDynamics_INCLUDE_DIRS "${SimDynamics_SimoxDir}" "${SimDynamics_DIR}" PARENT_SCOPE)
+ SET(SimDynamics_EXTERNAL_LIBRARIES "${SimDynamics_PHYSICS_LIBRARIES}" PARENT_SCOPE)
INCLUDE_DIRECTORIES(${SimDynamics_SimoxDir})
INCLUDE_DIRECTORIES(${SimDynamics_DIR})
@@ -128,7 +128,9 @@ if (SimDynamics_DYNAMICSENGINE)
)
INSTALL(DIRECTORY ${PROJECT_SOURCE_DIR} DESTINATION ${SimDynamics_INSTALL_HEADER_DIR}
- FILES_MATCHING PATTERN "*.h"
+ FILES_MATCHING
+ PATTERN "*.h"
+ PATTERN "*.hpp"
PATTERN ".svn" EXCLUDE
PATTERN "CMakeModules" EXCLUDE
PATTERN "tests" EXCLUDE
diff --git a/SimDynamics/CMakeModules/FindBullet.cmake b/SimDynamics/CMakeModules/FindBullet.cmake
index 9f41ad6985d035fe8c9f51ea53a1ed612ace1dc3..4ecafc7feda6f9ca92c14545cb4363d99ab57627 100644
--- a/SimDynamics/CMakeModules/FindBullet.cmake
+++ b/SimDynamics/CMakeModules/FindBullet.cmake
@@ -58,8 +58,8 @@ IF( BULLET_INCLUDE_DIR )
ENDIF( BULLET_INCLUDE_DIR )
MACRO( FIND_BULLET_LIBRARY_DIRNAME LIBNAME DIRNAME )
- UNSET( BULLET_${LIBNAME}_LIBRARY CACHE )
- UNSET( BULLET_${LIBNAME}_LIBRARY_debug CACHE )
+ #UNSET( BULLET_${LIBNAME}_LIBRARY CACHE )
+ #UNSET( BULLET_${LIBNAME}_LIBRARY_debug CACHE )
MARK_AS_ADVANCED( BULLET_${LIBNAME}_LIBRARY )
MARK_AS_ADVANCED( BULLET_${LIBNAME}_LIBRARY_debug )
FIND_LIBRARY( BULLET_${LIBNAME}_LIBRARY
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
index 3de0590b5381e0e7520e0433f113a0d555c683c7..9e0689fb2b2032f79d8095556ca0e3afd2ac450f 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
@@ -107,6 +107,8 @@ BulletObject::BulletObject(VirtualRobot::SceneObjectPtr o, SimulationType type)
rigidBody->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
cout << "TEST3" << endl;
#endif
+
+ setPoseIntern(o->getGlobalPose());
}
BulletObject::~BulletObject()
@@ -177,17 +179,20 @@ void BulletObject::setPosition( const Eigen::Vector3f &posMM )
setPose(pose);
}
-void BulletObject::setPose( const Eigen::Matrix4f &pose )
+void BulletObject::setPoseIntern( const Eigen::Matrix4f &pose )
{
- DynamicsObject::setPose(pose);
-
- // notify motionState
- motionState->setGlobalPose(pose);
+ // notify motionState
+ motionState->setGlobalPose(pose);
- // notify bullet object
- btTransform btT = BulletEngine::getPoseBullet(pose);
- rigidBody->setWorldTransform(btT);
+ // notify bullet object
+ btTransform btT = BulletEngine::getPoseBullet(pose);
+ rigidBody->setWorldTransform(btT);
+}
+void BulletObject::setPose( const Eigen::Matrix4f &pose )
+{
+ DynamicsObject::setPose(pose);
+ setPoseIntern(pose);
}
Eigen::Vector3f BulletObject::getLinearVelocity()
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
index e2417ac2a3215dc2bf7c768b7b00f5684b6b415f..0c7f85621e37ac33d4a481c504fe4adccbde43e9 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
@@ -70,6 +70,8 @@ public:
Eigen::Matrix4f getComGlobal();
protected:
+ void setPoseIntern(const Eigen::Matrix4f &pose);
+
btConvexHullShape* createConvexHullShape(VirtualRobot::TriMeshModelPtr trimesh);
boost::shared_ptr<btRigidBody> rigidBody;
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index f40ec63fd618df7d6c527eb1a3172d8208ad71fd..1c5ed7433ebd372b38b72086b5e566d3d6a00d78 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -454,11 +454,12 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr bodyA, VirtualRobot::Ro
btScalar diff = joint->getJointValueOffset();//startAngleBT + startAngle);
limMinBT = limMin + diff;//diff - limMax;//
limMaxBT = limMax + diff;//diff - limMin;//
- if (fabs(startAngleBT - startAngle)>1e-6)
+ // what does it mean if there are different startAngles?!
+ /*if (fabs(startAngleBT - startAngle)>1e-6)
{
cout << "joint " << joint->getName() << ": jv diff:" << diff << endl;
cout << "Simox limits: " << limMin << "/" << limMax << ", bullet limits:" << limMinBT << "/" << limMaxBT << endl;
- }
+ }*/
hinge->setLimit(btScalar(limMinBT),btScalar(limMaxBT));
vr2bulletOffset = diff;
//hinge->setLimit(btScalar(limMin),btScalar(limMax));
@@ -917,15 +918,25 @@ void BulletRobot::actuateJoints(float dt)
VR_ASSERT(dof);
btRotationalLimitMotor *m = dof->getRotationalLimitMotor(0);
VR_ASSERT(m);
- if (it->second.enabled)
+ switch (it->second.actuation)
{
- btScalar targ = btScalar(it->second.jointValueTarget+link.jointValueOffset);
- //btScalar act = btScalar(it->first->getJointValue());
- btScalar act = btScalar(getJointAngle(it->first));
- m->m_enableMotor = true;
- m->m_targetVelocity = (targ-act); // inverted joint dir?!
- } else
- m->m_enableMotor = false;
+ case ePosition:
+ {
+ btScalar targ = btScalar(it->second.jointValueTarget+link.jointValueOffset);
+ //btScalar act = btScalar(it->first->getJointValue());
+ btScalar act = btScalar(getJointAngle(it->first));
+ m->m_enableMotor = true;
+ m->m_targetVelocity = (targ-act); // inverted joint dir?!
+ }
+ break;
+ case eVelocity:
+ {
+ m->m_enableMotor = true;
+ m->m_targetVelocity = it->second.jointVelocityTarget;
+ }
+ default:
+ m->m_enableMotor = false;
+ }
#else
boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
if (!hinge)
@@ -943,27 +954,50 @@ void BulletRobot::actuateJoints(float dt)
m = hinge2->getRotationalLimitMotor(2); // third motor
}
VR_ASSERT(m);
- if (it->second.enabled)
+ switch (it->second.actuation)
{
- btScalar targ = btScalar(it->second.jointValueTarget+link.jointValueOffset);
- //btScalar act = btScalar(it->first->getJointValue());
- btScalar act = btScalar(getJointAngle(it->first));
- m->m_enableMotor = true;
- m->m_targetVelocity = (targ-act); // inverted joint dir?!
- } else
+ case ePosition:
+ {
+ btScalar targ = btScalar(it->second.jointValueTarget+link.jointValueOffset);
+ //btScalar act = btScalar(it->first->getJointValue());
+ btScalar act = btScalar(getJointAngle(it->first));
+ m->m_enableMotor = true;
+ m->m_targetVelocity = (targ-act);
+ }
+ break;
+
+ case eVelocity:
+ {
+ m->m_enableMotor = true;
+ m->m_targetVelocity = it->second.jointVelocityTarget;
+ }
+ break;
+
+ default:
m->m_enableMotor = false;
+ }
} else
{
- if (it->second.enabled)
- {
- btScalar targ = btScalar(it->second.jointValueTarget+link.jointValueOffset);
- //btScalar act = btScalar(it->first->getJointValue());
- btScalar act = btScalar(getJointAngle(it->first));
- hinge->enableAngularMotor(true,(targ-act),bulletMaxMotorImulse);
- //hinge->enableMotor(true);
- //hinge->setMotorTarget(it->second.jointValueTarget+link.jointValueOffset,dt);
- } else
- hinge->enableMotor(false);
+ switch (it->second.actuation)
+ {
+ case ePosition:
+ {
+ btScalar targ = btScalar(it->second.jointValueTarget+link.jointValueOffset);
+ //btScalar act = btScalar(it->first->getJointValue());
+ btScalar act = btScalar(getJointAngle(it->first));
+ hinge->enableAngularMotor(true,(targ-act),10.0f);
+ //hinge->enableMotor(true);
+ //hinge->setMotorTarget(it->second.jointValueTarget+link.jointValueOffset,dt);
+ }
+ break;
+ case eVelocity:
+ {
+ hinge->enableAngularMotor(true,it->second.jointVelocityTarget,10.0f);
+ }
+ break;
+ default:
+ hinge->enableMotor(false);
+ }
}
#endif
}
@@ -1040,12 +1074,65 @@ void BulletRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointValue
{
hinge->enableAngularMotor(true,0.0f,bulletMaxMotorImulse);// is max impulse ok?! (10 seems to be ok, 1 oscillates)
}
- DynamicsRobot::actuateNode(node,jointValue); // inverted joint direction in bullet
+ DynamicsRobot::actuateNode(node,jointValue);
#endif
} else
{
VR_ERROR << "Only Revolute joints implemented so far..." << endl;
- }
+ }
+}
+
+void BulletRobot::actuateNodeVel(RobotNodePtr node, float jointVelocity)
+{
+ VR_ASSERT(node);
+
+ if (node->isRotationalJoint())
+ {
+ if (!hasLink(node))
+ {
+ VR_ERROR << "No link for node " << node->getName() << endl;
+ return;
+ }
+ LinkInfo link = getLink(node);
+#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
+ boost::shared_ptr<btGeneric6DofConstraint> dof = boost::dynamic_pointer_cast<btGeneric6DofConstraint>(link.joint);
+ VR_ASSERT(dof);
+ btRotationalLimitMotor *m = dof->getRotationalLimitMotor(0);
+ VR_ASSERT(m);
+ m->m_enableMotor = true;
+ m->m_maxMotorForce = 5;//bulletMaxMotorImulse; //?!
+ m->m_maxLimitForce = 300;
+ DynamicsRobot::actuateNodeVel(node,jointVelocity);
+#else
+ boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
+ if (!hinge)
+ {
+ // hinge2 / universal joint
+ boost::shared_ptr<btUniversalConstraint> hinge2 = boost::dynamic_pointer_cast<btUniversalConstraint>(link.joint);
+ VR_ASSERT(hinge2);
+ btRotationalLimitMotor *m;
+ if (node==link.nodeJoint)
+ {
+ m = hinge2->getRotationalLimitMotor(1); // second motor
+ } else
+ {
+ VR_ASSERT(node==link.nodeJoint2);
+ m = hinge2->getRotationalLimitMotor(2); // third motor
+ }
+ VR_ASSERT(m);
+ m->m_enableMotor = true;
+ m->m_maxMotorForce = 5;//bulletMaxMotorImulse; //?!
+ m->m_maxLimitForce = 300;
+ } else
+ {
+ //hinge->enableAngularMotor(true,jointVelocity,bulletMaxMotorImulse);// is max impulse ok?! (10 seems to be ok, 1 oscillates)
+ }
+ DynamicsRobot::actuateNodeVel(node,jointVelocity); // inverted joint direction in bullet
+#endif
+ } else
+ {
+ VR_ERROR << "Only Revolute joints implemented so far..." << endl;
+ }
}
float BulletRobot::getJointAngle( VirtualRobot::RobotNodePtr rn )
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
index 430eff56b1ff83330c6b58657a504ceed697e68c..1184521e1fadf609842df47f27ccd9f6b4ba815f 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
@@ -72,7 +72,8 @@ public:
std::vector<LinkInfo> getLinks();
- virtual void actuateNode(VirtualRobot::RobotNodePtr node, float jointValue);
+ virtual void actuateNode(VirtualRobot::RobotNodePtr node, float jointValue);
+ virtual void actuateNodeVel(VirtualRobot::RobotNodePtr node, float jointVelocity);
/*!
Usually this method is called by the framework in every tick to perform joint actuation.
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
index e0c241b70321cfdfb0376022acdcac2bf2802b70..d227088470e9be23e7b4bea297e1beeb61470dd0 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
@@ -67,6 +67,12 @@ DynamicsObjectPtr DynamicsRobot::getDynamicsRobotNode( VirtualRobot::RobotNodePt
return dynamicRobotNodes[node];
}
+void DynamicsRobot::actuateNode( std::string &node, float jointValue )
+{
+ VR_ASSERT(robot);
+ VR_ASSERT(robot->hasRobotNode(node));
+ actuateNode(robot->getRobotNode(node),jointValue);
+}
void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointValue )
{
@@ -81,13 +87,42 @@ void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointVal
// createDynamicsNode(node);
- robotNodeActuationTarget target;
- target.enabled = true;
- target.node = node;
- target.jointValueTarget = jointValue;
- //target.dynNode = dnyRN;
+ robotNodeActuationTarget target;
+ target.actuation = ePosition;
+ target.node = node;
+ target.jointValueTarget = jointValue;
+ target.jointVelocityTarget = 0.0f;
+ //target.dynNode = dnyRN;
+
+ actuationTargets[node] = target;
+}
+
+void DynamicsRobot::actuateNodeVel( std::string &node, float jointVelocity )
+{
+ VR_ASSERT(robot);
+ VR_ASSERT(robot->hasRobotNode(node));
+ actuateNodeVel(robot->getRobotNode(node),jointVelocity);
+}
+
+void DynamicsRobot::actuateNodeVel( VirtualRobot::RobotNodePtr node, float jointVelocity )
+{
+ VR_ASSERT(robot);
+ VR_ASSERT(node);
+ VR_ASSERT(robot->hasRobotNode(node));
+
+ //if (!hasDynamicsRobotNode(node))
+ // createDynamicsNode(node);
+
+ //DynamicsObjectPtr dnyRN = getDynamicsRobotNode(node);
+
+ robotNodeActuationTarget target;
+ target.actuation = eVelocity;
+ target.node = node;
+ target.jointValueTarget = 0.0f;
+ target.jointVelocityTarget = jointVelocity;
+ //target.dynNode = dnyRN;
- actuationTargets[node] = target;
+ actuationTargets[node] = target;
}
void DynamicsRobot::disableNodeActuation( VirtualRobot::RobotNodePtr node )
@@ -97,12 +132,12 @@ void DynamicsRobot::disableNodeActuation( VirtualRobot::RobotNodePtr node )
actuationTargets.erase(node);
}
}
-void DynamicsRobot::enableActuation()
+void DynamicsRobot::enableActuation(JointActuation mode)
{
std::map<VirtualRobot::RobotNodePtr, robotNodeActuationTarget>::iterator it = actuationTargets.begin();
while (it!=actuationTargets.end())
{
- it->second.enabled = true;
+ it->second.actuation = mode;
it++;
}
}
@@ -111,7 +146,7 @@ void DynamicsRobot::disableActuation()
std::map<VirtualRobot::RobotNodePtr, robotNodeActuationTarget>::iterator it = actuationTargets.begin();
while (it!=actuationTargets.end())
{
- it->second.enabled = false;
+ it->second.actuation = eDisabled;
it++;
}
}
@@ -125,15 +160,15 @@ bool DynamicsRobot::isNodeActuated( VirtualRobot::RobotNodePtr node )
VR_ASSERT(node);
if (actuationTargets.find(node) == actuationTargets.end())
return false;
- return actuationTargets[node].enabled;
+ return actuationTargets[node].actuation!=eDisabled;
}
float DynamicsRobot::getNodeTarget( VirtualRobot::RobotNodePtr node )
{
- VR_ASSERT(node);
- if (actuationTargets.find(node) == actuationTargets.end())
- return 0.0f;
- return actuationTargets[node].jointValueTarget;
+ VR_ASSERT(node);
+ if (actuationTargets.find(node) == actuationTargets.end())
+ return 0.0f;
+ return actuationTargets[node].jointValueTarget;
}
@@ -155,6 +190,21 @@ Eigen::Matrix4f DynamicsRobot::getComGlobal( VirtualRobot::RobotNodePtr rn )
return com;
}
+void DynamicsRobot::setGlobalPose(Eigen::Matrix4f &gp)
+{
+ Eigen::Matrix4f currentPose = robot->getGlobalPose();
+ Eigen::Matrix4f delta = gp * currentPose.inverse();
+
+ robot->setGlobalPose(gp);
+ std::map<VirtualRobot::RobotNodePtr, DynamicsObjectPtr>::iterator it = dynamicRobotNodes.begin();
+ while (it != dynamicRobotNodes.end())
+ {
+ Eigen::Matrix4f newPose = it->second->getSceneObject()->getGlobalPose() * delta;
+ it->second->setPose(newPose);
+ it++;
+ }
+}
+
/*
void DynamicsRobot::setPose( const Eigen::Matrix4f &pose )
{
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.h b/SimDynamics/DynamicsEngine/DynamicsRobot.h
index 7f3b47fd62fcb9fb83bd4b0eca88c10f5447528d..d3cb6503527cf5a98b455d5a93b654dd2f1f0bc5 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.h
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.h
@@ -56,15 +56,24 @@ public:
*/
DynamicsObjectPtr getDynamicsRobotNode(VirtualRobot::RobotNodePtr node);
+ enum JointActuation
+ {
+ eDisabled,
+ ePosition,
+ eVelocity
+ };
/*!
Enable joint actuation for given node.
*/
- virtual void actuateNode(VirtualRobot::RobotNodePtr node, float jointValue);
- virtual void disableNodeActuation(VirtualRobot::RobotNodePtr node);
+ virtual void actuateNode(VirtualRobot::RobotNodePtr node, float jointValue);
+ virtual void actuateNodeVel(VirtualRobot::RobotNodePtr node, float jointVelocity);
+ virtual void actuateNode(std::string &node, float jointValue);
+ virtual void actuateNodeVel(std::string &node, float jointVelocity);
+ virtual void disableNodeActuation(VirtualRobot::RobotNodePtr node);
virtual bool isNodeActuated(VirtualRobot::RobotNodePtr node);
virtual float getNodeTarget(VirtualRobot::RobotNodePtr node);
- virtual void enableActuation();
+ virtual void enableActuation(JointActuation mode);
virtual void disableActuation();
/*!
@@ -88,16 +97,22 @@ public:
virtual Eigen::Matrix4f getComGlobal(VirtualRobot::RobotNodePtr rn);
+ virtual void setGlobalPose(Eigen::Matrix4f &gp);
+
+
+
protected:
virtual void createDynamicsNode(VirtualRobot::RobotNodePtr node);
+
struct robotNodeActuationTarget
{
- float jointValueTarget;
- VirtualRobot::RobotNodePtr node;
+ float jointValueTarget;
+ float jointVelocityTarget;
+ VirtualRobot::RobotNodePtr node;
//DynamicsObjectPtr dynNode; // if node is a joint without model, there is no dyn node!
- bool enabled;
+ JointActuation actuation;
};
std::map<VirtualRobot::RobotNodePtr, robotNodeActuationTarget> actuationTargets;
diff --git a/SimDynamics/config.cmake b/SimDynamics/config.cmake
index 8475ceb47fd002a7dd23a3e3eeb361ce79d7c7b0..3d523d30b4e97d35f7c1b61528965b1135db98ce 100644
--- a/SimDynamics/config.cmake
+++ b/SimDynamics/config.cmake
@@ -56,6 +56,9 @@ IF (NOT SimDynamics_CONFIGURED)
#MESSAGE (STATUS "BULLET_LIBRARIES: ${BULLET_LIBRARIES}")
#MESSAGE (STATUS "BULLET_OPENGL_INCLUDE_DIR: ${BULLET_OPENGL_INCLUDE_DIR}")
#MESSAGE (STATUS "BULLET_OpenGLSupport_LIBRARY_debug: ${BULLET_OpenGLSupport_LIBRARY_debug}")
+ SET(SIMDYNAMICS_INCLUDE_DIRS "${BULLET_INCLUDE_DIR}" "${BULLET_INCLUDE_DIR}/bullet" "${BULLET_DEMOS_INCLUDE_DIR}" "${BULLET_OPENGL_INCLUDE_DIR}" PARENT_SCOPE)
+
+
INCLUDE_DIRECTORIES(
${BULLET_INCLUDE_DIR}
${BULLET_INCLUDE_DIR}/bullet
@@ -66,6 +69,7 @@ IF (NOT SimDynamics_CONFIGURED)
OPTION(SimDynamics_USE_BULLET_USE_GLUT "Use Glut" ON)
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)
ENDIF( SimDynamics_USE_BULLET_DOUBLE_PRECISION )
SET (SimDynamics_PHYSICS_LIBRARIES "${BULLET_LIBRARIES}")
SET (SimDynamics_DYNAMICSENGINE TRUE)
@@ -83,7 +87,7 @@ IF (NOT SimDynamics_CONFIGURED)
IF (GLUT_FOUND)
MESSAGE("GLUT FOUND")
MESSAGE(${GLUT_glut_LIBRARY})
- SET(SimDynamics_PHYSICS_LIBRARIES ${SimDynamics_PHYSICS_LIBRARIES} ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY})
+ SET(SimDynamics_PHYSICS_LIBRARIES ${SimDynamics_PHYSICS_LIBRARIES} ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY})
ELSE (GLUT_FOUND)
SET( GLUT_glut_LIBRARY "" CACHE PATH "Glut library." )
#IF (MSVC)
@@ -99,14 +103,16 @@ IF (NOT SimDynamics_CONFIGURED)
IF (WIN32)
INCLUDE_DIRECTORIES(${GLUT_INCLUDE_DIR})
+ SET(SIMDYNAMICS_INCLUDE_DIRS "${SIMDYNAMICS_INCLUDE_DIRS}" "${GLUT_INCLUDE_DIR}" PARENT_SCOPE)
ELSE (WIN32)
# This is the lines for linux. This should always work if everything is installed and working fine.
INCLUDE_DIRECTORIES(/usr/include /usr/local/include ${GLUT_INCLUDE_DIR})
+ SET(SIMDYNAMICS_INCLUDE_DIRS "${SIMDYNAMICS_INCLUDE_DIRS}" "/usr/include" "/usr/local/include" "${GLUT_INCLUDE_DIR}")
ENDIF (WIN32)
ENDIF(SimDynamics_USE_BULLET_USE_GLUT)
-
- if (SimDynamics_USE_BULLET_USE_GLUT AND GLUT_glut_LIBRARY AND DEFINED BULLET_OpenGLSupport_LIBRARY)
+ MESSAGE ("BULLET_OpenGLSupport_LIBRARY: ${BULLET_OpenGLSupport_LIBRARY}")
+ if (SimDynamics_USE_BULLET_USE_GLUT AND GLUT_glut_LIBRARY AND BULLET_OpenGLSupport_LIBRARY)
MESSAGE ("Enabling OpenGL / Glut support")
SET (SimDynamics_BULLET_OpenGL TRUE)
else()
@@ -123,7 +129,9 @@ IF (NOT SimDynamics_CONFIGURED)
endif()
-
+ MESSAGE ("**SIMDYNAMICS_INCLUDE_DIRS: ${SIMDYNAMICS_INCLUDE_DIRS}")
+ MESSAGE ("**SimDynamics_PHYSICS_LIBRARIES: ${SimDynamics_PHYSICS_LIBRARIES}")
+
#######################################################################
# Setup for testing
#######################################################################
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
index 63bca20af11e744ef63b968385f106738ed2d291..7ee96d4c9b1709bb91aea45ac62925d7af0920d2 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
@@ -159,7 +159,7 @@ void SimDynamicsWindow::actuation()
bool actuate = UI.checkBoxActuation->checkState() == Qt::Checked;
if (actuate)
- dynamicsRobot->enableActuation();
+ dynamicsRobot->enableActuation(SimDynamics::DynamicsRobot::ePosition);
else
dynamicsRobot->disableActuation();
}