From c7dd0dbc20c0028de227fd97d0de127c3be18db8 Mon Sep 17 00:00:00 2001
From: vahrenkamp <vahrenkamp@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Tue, 13 Aug 2013 13:37:26 +0000
Subject: [PATCH] Added optional SimualtionType flag (used by SimDynamics).
Extended grid visualization classes, texture files can now be specified.
Added several SimDynamics improvements.
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@405 042f3d55-54a8-47e9-b7fb-15903f145c44
---
.../BulletEngine/BulletCoinQtViewer.cpp | 16 ++-
.../BulletEngine/BulletCoinQtViewer.h | 8 +-
.../BulletEngine/BulletEngine.cpp | 30 +++--
.../BulletEngine/BulletEngine.h | 1 +
.../BulletEngine/BulletEngineFactory.cpp | 4 +-
.../BulletEngine/BulletEngineFactory.h | 2 +-
.../BulletEngine/BulletObject.cpp | 20 ++--
.../BulletEngine/BulletObject.h | 2 +-
.../BulletEngine/BulletRobot.cpp | 106 ++++++++----------
.../DynamicsEngine/BulletEngine/BulletRobot.h | 5 +-
.../BulletEngine/SimoxMotionState.cpp | 7 ++
SimDynamics/DynamicsEngine/DynamicsEngine.cpp | 12 +-
SimDynamics/DynamicsEngine/DynamicsEngine.h | 8 ++
.../DynamicsEngine/DynamicsEngineFactory.h | 4 +-
SimDynamics/DynamicsEngine/DynamicsObject.cpp | 9 +-
SimDynamics/DynamicsEngine/DynamicsObject.h | 11 +-
SimDynamics/DynamicsEngine/DynamicsRobot.cpp | 7 +-
SimDynamics/DynamicsEngine/DynamicsRobot.h | 3 +-
SimDynamics/DynamicsWorld.cpp | 4 +-
SimDynamics/DynamicsWorld.h | 6 +-
.../BulletDebugViewerGlut.cpp | 13 ++-
.../SimDynamicsViewer/SimDynamicsViewer.cpp | 2 +
.../SimDynamicsViewer/simDynamicsViewer.ui | 29 +++--
.../SimDynamicsViewer/simDynamicsWindow.cpp | 30 ++++-
.../SimDynamicsViewer/simDynamicsWindow.h | 1 +
VirtualRobot/SceneObject.cpp | 14 ++-
VirtualRobot/SceneObject.h | 50 ++++++++-
.../CoinVisualizationFactory.cpp | 22 ++--
.../CoinVisualizationFactory.h | 2 +-
VirtualRobot/XML/BaseIO.cpp | 15 +++
VirtualRobot/XML/ObjectIO.cpp | 29 ++++-
31 files changed, 330 insertions(+), 142 deletions(-)
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp
index efa423e3a..97185d58d 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp
@@ -108,14 +108,28 @@ void BulletCoinQtViewer::initSceneGraph( QFrame* embedViewer, SoNode* scene )
viewer->setAntialiasing(true, 4);
viewer->setGLRenderAction(new SoBoxHighlightRenderAction);
- viewer->setTransparencyType(SoGLRenderAction::BLEND);
+ viewer->setTransparencyType(SoGLRenderAction::SORTED_OBJECT_BLEND);
viewer->setFeedbackVisibility(true);
if (bulletEngine->getFloor())
{
+ // standard box visu:
+ /*
SceneObjectPtr so = bulletEngine->getFloor()->getSceneObject();
SoNode * n = CoinVisualizationFactory::getCoinVisualization(so,SceneObject::Full);
+ */
+
+ // better grid visu
+ Eigen::Vector3f floorPos;
+ Eigen::Vector3f floorUp;
+ float floorExtendMM;
+ float floorDepthMM;
+ bulletEngine->getFloorInfo(floorPos,floorUp,floorExtendMM,floorDepthMM);
+ SoNode * n = (SoNode*)CoinVisualizationFactory::CreatePlaneVisualization(floorPos,floorUp,floorExtendMM,0.0f);
if (n)
+ {
floor->addChild(n);
+ addedVisualizations[bulletEngine->getFloor()] = n;
+ }
//addVisualization(bulletEngine->getFloor());
}
if (scene)
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.h
index 9085fde04..a3349b69b 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.h
@@ -103,7 +103,8 @@ protected:
It can be overwritten in order to perform custom updates.
It is safe to access the scene graph.
*/
- virtual void customUpdate(){};
+ virtual void customUpdate(){}
+
/*!
This method is called when a node has been selected by the user.
It can be overwritten to implement custom reactions.
@@ -113,11 +114,12 @@ protected:
virtual void customSelection(SoPath *path)
{
std::cout << "Selecting node " << path->getTail()->getTypeId().getName().getString() << endl;
- };
+ }
+
virtual void customDeselection(SoPath *path)
{
std::cout << "Deselecting node " << path->getTail()->getTypeId().getName().getString() << endl;
- };
+ }
//! Redraw
virtual void scheduleRedraw();
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
index 2534e15ee..f434ac97b 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
@@ -130,18 +130,24 @@ bool BulletEngine::addObject( DynamicsObjectPtr o )
return false;
}
int btColFlag;
- switch (o->getSimType())
+ switch (o->getSimType())
{
- case DynamicsObject::eStatic:
+ case VirtualRobot::SceneObject::Physics::eStatic:
btColFlag = btCollisionObject::CF_STATIC_OBJECT;
- break;
- case DynamicsObject::eKinematic:
+ break;
+ case VirtualRobot::SceneObject::Physics::eKinematic:
btColFlag = btCollisionObject::CF_KINEMATIC_OBJECT;
break;
- case DynamicsObject::eDynamic:
- btColFlag = 0;
+ case VirtualRobot::SceneObject::Physics::eDynamic:
+ case VirtualRobot::SceneObject::Physics::eUnknown:
+ btColFlag = 0;
break;
- }
+ default:
+ // Dynamic Object
+ btColFlag = 0;
+ break;
+
+ }
btObject->getRigidBody()->setCollisionFlags(btColFlag);
btObject->getRigidBody()->setRestitution(bulletConfig->bulletObjectRestitution);
btObject->getRigidBody()->setFriction(bulletConfig->bulletObjectFriction);
@@ -191,8 +197,8 @@ void BulletEngine::createFloorPlane( const Eigen::Vector3f &pos, const Eigen::Ve
{
boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
DynamicsEngine::createFloorPlane(pos,up);
- float size = 50000.0f; // mm
- float sizeSmall = 500.0f;
+ float size = floorExtendMM;//50000.0f; // mm
+ float sizeSmall = floorDepthMM; 500.0f;
float w = size;
float h = size;
float d = sizeSmall;
@@ -215,7 +221,11 @@ void BulletEngine::createFloorPlane( const Eigen::Vector3f &pos, const Eigen::Ve
gp.setIdentity();
gp(2,3) = -sizeSmall*0.5f;
groundObject->setGlobalPose(gp);
- BulletObjectPtr groundObjectBt(new BulletObject(groundObject,DynamicsObject::eStatic));
+
+ groundObject->getVisualization();
+ groundObject->setSimulationType(VirtualRobot::SceneObject::Physics::eStatic);
+
+ BulletObjectPtr groundObjectBt(new BulletObject(groundObject));
floor = groundObjectBt;
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
index 8892286f8..3a57b4d97 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
@@ -23,6 +23,7 @@
#ifndef _SimDynamics_BulletEngine_h_
#define _SimDynamics_BulletEngine_h_
+#include <VirtualRobot/SceneObject.h>
#include "../DynamicsEngine.h"
#include "BulletRobot.h"
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.cpp
index b93348a5d..6e1d54a46 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.cpp
@@ -44,9 +44,9 @@ DynamicsEnginePtr BulletEngineFactory::createEngine(DynamicsEngineConfigPtr conf
return bulletEngine;
}
-DynamicsObjectPtr BulletEngineFactory::createObject( VirtualRobot::SceneObjectPtr o, DynamicsObject::SimulationType simType /*= DynamicsObject::eDynamic*/ )
+DynamicsObjectPtr BulletEngineFactory::createObject( VirtualRobot::SceneObjectPtr o )
{
- return BulletObjectPtr(new BulletObject(o,simType));
+ return BulletObjectPtr(new BulletObject(o));
}
SimDynamics::DynamicsRobotPtr BulletEngineFactory::createRobot( VirtualRobot::RobotPtr robot )
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.h
index f52f210f8..f1d75d775 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.h
@@ -44,7 +44,7 @@ public:
virtual DynamicsEnginePtr createEngine(DynamicsEngineConfigPtr config = DynamicsEngineConfigPtr());
- virtual DynamicsObjectPtr createObject(VirtualRobot::SceneObjectPtr o, DynamicsObject::SimulationType simType = DynamicsObject::eDynamic);
+ virtual DynamicsObjectPtr createObject(VirtualRobot::SceneObjectPtr o);
virtual DynamicsRobotPtr createRobot(VirtualRobot::RobotPtr robot);
// AbstractFactoryMethod
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
index 9e0689fb2..444cfae47 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
@@ -18,8 +18,8 @@ using namespace VirtualRobot;
namespace SimDynamics {
-BulletObject::BulletObject(VirtualRobot::SceneObjectPtr o, SimulationType type)
- : DynamicsObject(o, type)
+BulletObject::BulletObject(VirtualRobot::SceneObjectPtr o)
+ : DynamicsObject(o)
{
float interatiaFactor = 1.0f;
#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
@@ -55,7 +55,7 @@ BulletObject::BulletObject(VirtualRobot::SceneObjectPtr o, SimulationType type)
btScalar mass = o->getMass();
btVector3 localInertia;
- if (mass<=0 && type==eDynamic)
+ if (mass<=0 && (o->getSimulationType()==VirtualRobot::SceneObject::Physics::eDynamic || o->getSimulationType()==VirtualRobot::SceneObject::Physics::eUnknown))
{
//THROW_VR_EXCEPTION ("mass == 0 -> SimulationType must not be eDynamic! ");
mass = btScalar(1.0f); // give object a dummy mass
@@ -74,7 +74,7 @@ BulletObject::BulletObject(VirtualRobot::SceneObjectPtr o, SimulationType type)
mass = 0;
localInertia.setValue(0.0f, 0.0f, 0.0f);
#else
- if (type != eDynamic) {
+ if (o->getSimulationType()!=VirtualRobot::SceneObject::Physics::eDynamic && o->getSimulationType()!=VirtualRobot::SceneObject::Physics::eUnknown) {
mass = 0;
localInertia.setValue(0.0f, 0.0f, 0.0f);
} else
@@ -108,7 +108,7 @@ BulletObject::BulletObject(VirtualRobot::SceneObjectPtr o, SimulationType type)
cout << "TEST3" << endl;
#endif
- setPoseIntern(o->getGlobalPose());
+ setPoseIntern(o->getGlobalPoseVisualization());
}
BulletObject::~BulletObject()
@@ -127,8 +127,8 @@ btConvexHullShape* BulletObject::createConvexHullShape(VirtualRobot::TriMeshMode
Eigen::Matrix4f comLoc;
comLoc.setIdentity();
- comLoc.block(0,3,3,1) = sceneObject->getCoMGlobal();
- comLoc = (sceneObject->getGlobalPoseVisualization().inverse()*comLoc);
+ comLoc.block(0,3,3,1) = sceneObject->getCoMGlobal();
+ comLoc = (sceneObject->getGlobalPoseVisualization().inverse()*comLoc);
com = comLoc.block(0,3,3,1);
float sc = 1.0f;
@@ -183,10 +183,6 @@ void BulletObject::setPoseIntern( const Eigen::Matrix4f &pose )
{
// notify motionState
motionState->setGlobalPose(pose);
-
- // notify bullet object
- btTransform btT = BulletEngine::getPoseBullet(pose);
- rigidBody->setWorldTransform(btT);
}
void BulletObject::setPose( const Eigen::Matrix4f &pose )
@@ -206,7 +202,7 @@ Eigen::Vector3f BulletObject::getAngularVelocity()
{
if (!rigidBody)
return Eigen::Vector3f::Zero();
- return (BulletEngine::getVecEigen(rigidBody->getAngularVelocity()));
+ return (BulletEngine::getVecEigen(rigidBody->getAngularVelocity(),false));
}
void BulletObject::setLinearVelocity( const Eigen::Vector3f &vel )
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
index 0c7f85621..17e8171a7 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
@@ -38,7 +38,7 @@ public:
/*!
Constructor
*/
- BulletObject(VirtualRobot::SceneObjectPtr o, SimulationType type = eDynamic);
+ BulletObject(VirtualRobot::SceneObjectPtr o);
/*!
*/
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index 0dc1d6999..2a093a466 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -441,8 +441,9 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr bodyA, VirtualRobot::Ro
boost::shared_ptr<btHingeConstraint> hinge(new btHingeConstraint(*btBody1, *btBody2, tr1, tr2, true));
+
// todo: check effects of parameters...
- //hinge->setParam(BT_CONSTRAINT_STOP_ERP,0.9f);
+ hinge->setParam(BT_CONSTRAINT_STOP_ERP,0.9f);
/*
hinge->setParam(BT_CONSTRAINT_CFM,0.9f);
hinge->setParam(BT_CONSTRAINT_STOP_CFM,0.01f);*/
@@ -465,6 +466,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr bodyA, VirtualRobot::Ro
vr2bulletOffset = diff;
//hinge->setLimit(btScalar(limMin),btScalar(limMax));
//hinge->setAngularOnly(true);
+
jointbt = hinge;
}
} else
@@ -518,7 +520,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr bodyA, VirtualRobot::Ro
}
#endif
}
-
+/*
void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::RobotNodePtr node2, bool enableJointMotors )
{
THROW_VR_EXCEPTION_IF (!(node1->hasChild(node2)),"node1 must be parent of node2");
@@ -593,7 +595,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
if (node2->isTranslationalJoint())
{
VR_WARNING << "translational joint nyi, creating a fixed link..." << endl;
- }
+ }rnRevJoint->getJointRotationAxis();
bool createJoint = node2->isRotationalJoint();
if (createJoint)
{
@@ -630,15 +632,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
btVector3 pivot1 = BulletEngine::getVecBullet(anchor_inNode1.block(0,3,3,1));
btVector3 pivot2 = BulletEngine::getVecBullet(anchor_inNode2.block(0,3,3,1));
-
-
- /*pivTr1.setIdentity();
- pivTr1.setOrigin(pivot1);
- btTransform pivTr2;
- pivTr2.setIdentity();
- pivTr2.setOrigin(pivot2);
- btTransform pivotTest1 = btBody1->getWorldTransform()*pivTr1;
- btTransform pivotTest2 = btBody2->getWorldTransform()*pivTr2;*/
+
float limMin,limMax;
limMin = node2->getJointLimitLo();
@@ -683,7 +677,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
{
cout << "joint " << node2->getName() << ": jv diff:" << diff << endl;
cout << "Simox limits: " << limMin << "/" << limMax << ", bullet limits:" << limMinBT << "/" << limMaxBT << endl;
- }*/
+ }* /
dof->setLimit(0,0,0);
dof->setLimit(1,0,0);
dof->setLimit(2,0,0);
@@ -703,9 +697,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
boost::shared_ptr<btHingeConstraint> hinge(new btHingeConstraint(*btBody1, *btBody2, pivot1, pivot2, axis1, axis2,false));
//hinge->setParam(BT_CONSTRAINT_STOP_ERP,0.9f);
- /*
- hinge->setParam(BT_CONSTRAINT_CFM,0.9f);
- hinge->setParam(BT_CONSTRAINT_STOP_CFM,0.01f);*/
+
//hinge->setLimit(limMin,limMax);//,btScalar(1.0f));
//hinge->setParam(BT_CONSTRAINT_CFM,1.0f);
@@ -738,7 +730,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
btVector3 axis1 = BulletEngine::getVecBullet(axis_inLocal1,false);
btVector3 axis2 = BulletEngine::getVecBullet(axis_inLocal2,false);
joint.reset(new btSliderConstraint(*btBody1, *btBody2, axis1, axis2, true));
- } else*/
+ } else* /
{
// fixed joint
@@ -796,28 +788,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
for (int i=0;i<6;i++)
generic6Dof->setLimit(i,0,0);
- /*btVector3 limitZero(0,0,0);
- generic6Dof->setAngularLowerLimit(limitZero);
- generic6Dof->setAngularUpperLimit(limitZero);*/
- /*generic6Dof->setParam(BT_CONSTRAINT_STOP_CFM,0.01f,0);
- generic6Dof->setParam(BT_CONSTRAINT_STOP_CFM,0.01f,1);
- generic6Dof->setParam(BT_CONSTRAINT_STOP_CFM,0.01f,2);
- generic6Dof->setParam(BT_CONSTRAINT_STOP_CFM,0.01f,3);
- generic6Dof->setParam(BT_CONSTRAINT_STOP_CFM,0.01f,4);
- generic6Dof->setParam(BT_CONSTRAINT_STOP_CFM,0.01f,5);*/
- /*generic6Dof->setParam(BT_CONSTRAINT_STOP_ERP,0.9f,0);
- generic6Dof->setParam(BT_CONSTRAINT_STOP_ERP,0.9f,1);
- generic6Dof->setParam(BT_CONSTRAINT_STOP_ERP,0.9f,2);
- generic6Dof->setParam(BT_CONSTRAINT_STOP_ERP,0.9f,3);
- generic6Dof->setParam(BT_CONSTRAINT_STOP_ERP,0.9f,4);
- generic6Dof->setParam(BT_CONSTRAINT_STOP_ERP,0.9f,5);*/
- /*btRotationalLimitMotor *r = generic6Dof->getRotationalLimitMotor(0);
- r->m_maxLimitForce = 1000.0f;
- r = generic6Dof->getRotationalLimitMotor(1);
- r->m_maxLimitForce = 1000.0f;
- r = generic6Dof->getRotationalLimitMotor(2);
- r->m_maxLimitForce = 1000.0f;*/
joint = generic6Dof;
#endif
}
@@ -865,7 +836,7 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
}
#endif
}
-
+*/
bool BulletRobot::hasLink( VirtualRobot::RobotNodePtr node1, VirtualRobot::RobotNodePtr node2 )
{
@@ -1184,25 +1155,18 @@ float BulletRobot::getJointAngle( VirtualRobot::RobotNodePtr rn )
#endif
}
-float BulletRobot::getJointSpeed( VirtualRobot::RobotNodePtr rn )
+float BulletRobot::getJointTargetSpeed( VirtualRobot::RobotNodePtr rn )
{
- VR_ASSERT(rn);
- if (!hasLink(rn))
- {
- VR_ERROR << "No link with node " << rn->getName() << endl;
- return 0.0f;
- }
- LinkInfo link = getLink(rn);
-#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
- boost::shared_ptr<btGeneric6DofConstraint> dof = boost::dynamic_pointer_cast<btGeneric6DofConstraint>(link.joint);
- VR_ASSERT(dof);
- btRotationalLimitMotor *m = dof->getRotationalLimitMotor(0);
- VR_ASSERT(m);
- return m->m_targetVelocity;
-#else
- boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
- if (!hinge)
- {
+ VR_ASSERT(rn);
+ if (!hasLink(rn))
+ {
+ VR_ERROR << "No link with node " << rn->getName() << endl;
+ return 0.0f;
+ }
+ LinkInfo link = getLink(rn);
+ 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);
if (!hinge2)
@@ -1218,8 +1182,34 @@ float BulletRobot::getJointSpeed( VirtualRobot::RobotNodePtr rn )
return 0.0f;
VR_ASSERT(m);
return m->m_targetVelocity;
- }
+ }
return hinge->getMotorTargetVelosity();
+}
+
+float BulletRobot::getJointSpeed( VirtualRobot::RobotNodePtr rn )
+{
+ VR_ASSERT(rn);
+ if (!hasLink(rn))
+ {
+ VR_ERROR << "No link with node " << rn->getName() << endl;
+ return 0.0f;
+ }
+ LinkInfo link = getLink(rn);
+#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
+ VR_WARNING << "NYI" << endl;
+ return 0.0;
+#else
+ boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
+ if (!hinge)
+ {
+ VR_WARNING << "NYI" << endl;
+ return 0.0;
+ }
+
+ 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());
+ return speed;//hinge->getMotorTargetVelosity();
#endif
}
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
index 1943f6458..bae546c1f 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
@@ -82,8 +82,9 @@ public:
virtual void actuateJoints(float dt);
virtual float getJointAngle(VirtualRobot::RobotNodePtr rn);
- virtual float getJointSpeed(VirtualRobot::RobotNodePtr rn);
- virtual float getNodeTarget(VirtualRobot::RobotNodePtr node);
+ virtual float getJointSpeed(VirtualRobot::RobotNodePtr rn);
+ virtual float getJointTargetSpeed(VirtualRobot::RobotNodePtr rn);
+ virtual float getNodeTarget(VirtualRobot::RobotNodePtr node);
/*!
Returns the CoM pose, which is reported by bullet
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
index 8659a06c6..35ae8ce0d 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
@@ -121,6 +121,13 @@ void SimoxMotionState::setGlobalPoseSimox( const Eigen::Matrix4f& worldPose )
// we assume that all models are handled by Bullet, so we do not need to update children
robotNodeActuator->updateVisualizationPose(resPose, ja, false);
+#if 0
+ if (rn->getName() == "Shoulder 1 L")
+ {
+ cout << "Shoulder 1 L:" << ja << ", speed:" << bdr->getJointSpeed(rn) << endl;
+ }
+
+#endif
} /*else
{
VR_WARNING << "Could not determine dynamic robot?!" << endl;
diff --git a/SimDynamics/DynamicsEngine/DynamicsEngine.cpp b/SimDynamics/DynamicsEngine/DynamicsEngine.cpp
index 43142461c..06ad319f9 100644
--- a/SimDynamics/DynamicsEngine/DynamicsEngine.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsEngine.cpp
@@ -8,6 +8,8 @@ DynamicsEngine::DynamicsEngine()
{
floorPos.setZero();
floorUp.setZero();
+ floorDepthMM = 500.0f;
+ floorExtendMM = 50000.0f;
}
DynamicsEngine::~DynamicsEngine()
@@ -143,7 +145,15 @@ bool DynamicsEngine::checkCollisionEnabled( DynamicsObject* o1 )
{
boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
std::vector<DynamicsObject*>::iterator i1 = find(collisionToAllDisabled.begin(),collisionToAllDisabled.end(),o1);
- return (i1 == collisionToAllDisabled.end());
+ return (i1 == collisionToAllDisabled.end());
+}
+
+void DynamicsEngine::getFloorInfo(Eigen::Vector3f &floorPos, Eigen::Vector3f &floorUp, float &floorExtendMM, float &floorDepthMM)
+{
+ floorPos = this->floorPos;
+ floorUp = this->floorUp;
+ floorExtendMM = this->floorExtendMM;
+ floorDepthMM = this->floorDepthMM;
}
bool DynamicsEngine::checkCollisionEnabled( DynamicsObject* o1, DynamicsObject* o2 )
diff --git a/SimDynamics/DynamicsEngine/DynamicsEngine.h b/SimDynamics/DynamicsEngine/DynamicsEngine.h
index 32167c4e7..2c5ca3f1d 100644
--- a/SimDynamics/DynamicsEngine/DynamicsEngine.h
+++ b/SimDynamics/DynamicsEngine/DynamicsEngine.h
@@ -124,6 +124,11 @@ public:
DynamicsObjectPtr getFloor(){return floor;}
+ /*!
+ Stores current floor description to floorPos and floorUp.
+ */
+ void getFloorInfo(Eigen::Vector3f &floorPos, Eigen::Vector3f &floorUp, float &floorExtendMM, float &floorDepthMM);
+
struct DynamicsContactInfo
{
DynamicsObject* objectA;
@@ -154,6 +159,9 @@ protected:
Eigen::Vector3f floorPos;
Eigen::Vector3f floorUp;
+ float floorExtendMM;
+ float floorDepthMM;
+
boost::recursive_mutex engineMutex;
};
diff --git a/SimDynamics/DynamicsEngine/DynamicsEngineFactory.h b/SimDynamics/DynamicsEngine/DynamicsEngineFactory.h
index 3d7491b99..ee55bafd3 100644
--- a/SimDynamics/DynamicsEngine/DynamicsEngineFactory.h
+++ b/SimDynamics/DynamicsEngine/DynamicsEngineFactory.h
@@ -48,9 +48,9 @@ public:
//! Derived classes must return the correct engine implementation.
virtual DynamicsEnginePtr createEngine(DynamicsEngineConfigPtr config = DynamicsEngineConfigPtr()){return DynamicsEnginePtr();}
- virtual DynamicsObjectPtr createObject(VirtualRobot::SceneObjectPtr o, DynamicsObject::SimulationType simType = DynamicsObject::eDynamic)
+ virtual DynamicsObjectPtr createObject(VirtualRobot::SceneObjectPtr o)
{
- return DynamicsObjectPtr(new DynamicsObject(o,simType));
+ return DynamicsObjectPtr(new DynamicsObject(o));
}
virtual DynamicsRobotPtr createRobot(VirtualRobot::RobotPtr robot)
{
diff --git a/SimDynamics/DynamicsEngine/DynamicsObject.cpp b/SimDynamics/DynamicsEngine/DynamicsObject.cpp
index 26be3674a..37b1b4379 100644
--- a/SimDynamics/DynamicsEngine/DynamicsObject.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsObject.cpp
@@ -4,11 +4,10 @@
namespace SimDynamics {
-DynamicsObject::DynamicsObject(VirtualRobot::SceneObjectPtr o, SimulationType type)
+DynamicsObject::DynamicsObject(VirtualRobot::SceneObjectPtr o)
{
THROW_VR_EXCEPTION_IF(!o,"NULL object");
sceneObject = o;
- simulationType = type;
}
DynamicsObject::~DynamicsObject()
@@ -20,14 +19,14 @@ std::string DynamicsObject::getName() const
return sceneObject->getName();
}
-DynamicsObject::SimulationType DynamicsObject::getSimType() const
+VirtualRobot::SceneObject::Physics::SimulationType DynamicsObject::getSimType() const
{
- return simulationType;
+ return sceneObject->getSimulationType();
}
void DynamicsObject::setPose( const Eigen::Matrix4f &pose )
{
- if (simulationType==eStatic)
+ if (sceneObject->getSimulationType()==VirtualRobot::SceneObject::Physics::eStatic)
{
VR_ERROR << "Could not move static object, aborting..." << endl;
return;
diff --git a/SimDynamics/DynamicsEngine/DynamicsObject.h b/SimDynamics/DynamicsEngine/DynamicsObject.h
index f2e9f41a4..595e23bc7 100644
--- a/SimDynamics/DynamicsEngine/DynamicsObject.h
+++ b/SimDynamics/DynamicsEngine/DynamicsObject.h
@@ -24,6 +24,7 @@
#define _SimDynamics_DynamicsObject_h_
#include "../SimDynamics.h"
+#include <VirtualRobot/SceneObject.h>
namespace SimDynamics
{
@@ -32,16 +33,16 @@ class SIMDYNAMICS_IMPORT_EXPORT DynamicsObject
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- enum SimulationType
+ /*enum SimulationType
{
eStatic, // cannot move, but collide
eKinematic, // can be moved, but no dynamics
eDynamic // full dynamic simulation
- };
+ };*/
/*!
Constructor
*/
- DynamicsObject(VirtualRobot::SceneObjectPtr o, SimulationType type = eDynamic);
+ DynamicsObject(VirtualRobot::SceneObjectPtr o);//, SimulationType type = eDynamic);
/*!
*/
@@ -49,7 +50,7 @@ public:
std::string getName() const;
- SimulationType getSimType() const;
+ VirtualRobot::SceneObject::Physics::SimulationType getSimType() const;
/*!
Set world position [MM].
@@ -69,7 +70,7 @@ public:
protected:
VirtualRobot::SceneObjectPtr sceneObject;
- SimulationType simulationType;
+ //SimulationType simulationType;
};
typedef boost::shared_ptr<DynamicsObject> DynamicsObjectPtr;
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
index d22708847..437b5a0f9 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
@@ -179,7 +179,12 @@ float DynamicsRobot::getJointAngle( VirtualRobot::RobotNodePtr rn )
float DynamicsRobot::getJointSpeed( VirtualRobot::RobotNodePtr rn )
{
- return 0.0f;
+ return 0.0f;
+}
+
+float DynamicsRobot::getJointTargetSpeed( VirtualRobot::RobotNodePtr rn )
+{
+ return 0.0f;
}
Eigen::Matrix4f DynamicsRobot::getComGlobal( VirtualRobot::RobotNodePtr rn )
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.h b/SimDynamics/DynamicsEngine/DynamicsRobot.h
index d3cb65035..b5760285e 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.h
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.h
@@ -93,7 +93,8 @@ public:
virtual float getJointAngle(VirtualRobot::RobotNodePtr rn);
- virtual float getJointSpeed(VirtualRobot::RobotNodePtr rn);
+ virtual float getJointSpeed(VirtualRobot::RobotNodePtr rn);
+ virtual float getJointTargetSpeed(VirtualRobot::RobotNodePtr rn);
virtual Eigen::Matrix4f getComGlobal(VirtualRobot::RobotNodePtr rn);
diff --git a/SimDynamics/DynamicsWorld.cpp b/SimDynamics/DynamicsWorld.cpp
index 3ef91fb21..80d3247e5 100644
--- a/SimDynamics/DynamicsWorld.cpp
+++ b/SimDynamics/DynamicsWorld.cpp
@@ -89,14 +89,14 @@ bool DynamicsWorld::removeObject( DynamicsObjectPtr o )
return engine->removeObject(o);
}
-DynamicsObjectPtr DynamicsWorld::CreateDynamicsObject( VirtualRobot::SceneObjectPtr o, DynamicsObject::SimulationType simType )
+DynamicsObjectPtr DynamicsWorld::CreateDynamicsObject( VirtualRobot::SceneObjectPtr o )
{
SIMDYNAMICS_ASSERT(o);
DynamicsEngineFactoryPtr factory = DynamicsEngineFactory::first(NULL);
SIMDYNAMICS_ASSERT(factory);
- return factory->createObject(o,simType);
+ return factory->createObject(o);
}
void DynamicsWorld::createFloorPlane( const Eigen::Vector3f &pos /*= Eigen::Vector3f(0,0,0)*/, const Eigen::Vector3f &up /*= Eigen::Vector3f(0,0,1.0f)*/ )
diff --git a/SimDynamics/DynamicsWorld.h b/SimDynamics/DynamicsWorld.h
index ecb20d8e1..909b88272 100644
--- a/SimDynamics/DynamicsWorld.h
+++ b/SimDynamics/DynamicsWorld.h
@@ -56,9 +56,11 @@ public:
virtual ~DynamicsWorld();
/*!
- Build a dynmic version of your VirtualRobot::SceneObject. This can be an Obstacle or a ManipulationObject.
+ Build a dynamic version of your VirtualRobot::SceneObject. This can be an Obstacle or a ManipulationObject.
+ Internally the value of o->getSimulationtype() is queried in order to specify which type of simulation should be performed.
+ If the simualtion type is not specified a full dynamic object is created (as with eDynamic).
*/
- static DynamicsObjectPtr CreateDynamicsObject(VirtualRobot::SceneObjectPtr o, DynamicsObject::SimulationType simType = DynamicsObject::eDynamic);
+ static DynamicsObjectPtr CreateDynamicsObject(VirtualRobot::SceneObjectPtr o);
/*!
Build a dynamic version of your VirtualRobot::Robot.
diff --git a/SimDynamics/examples/BulletDebugViewerGlut/BulletDebugViewerGlut.cpp b/SimDynamics/examples/BulletDebugViewerGlut/BulletDebugViewerGlut.cpp
index d433d67cd..621d32fc3 100644
--- a/SimDynamics/examples/BulletDebugViewerGlut/BulletDebugViewerGlut.cpp
+++ b/SimDynamics/examples/BulletDebugViewerGlut/BulletDebugViewerGlut.cpp
@@ -5,6 +5,7 @@
#include <VirtualRobot/Obstacle.h>
#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/XML/ObjectIO.h>
#include <VirtualRobot/RuntimeEnvironment.h>
using namespace std;
@@ -22,8 +23,9 @@ int main(int argc,char* argv[])
//std::string robFile("robots/examples/SimpleRobot/Joint5.xml");
//std::string robFile("robots/iCub/iCub.xml");
- std::string robFile("robots/ArmarIII/ArmarIII.xml");
- //std::string robFile("robots/iCub/iCub_RightHand.xml");
+ std::string robFile("robots/ArmarIII/ArmarIII.xml");
+ //std::string robFile("robots/ArmarIII/ArmarIII-RightArm.xml");
+ //std::string robFile("robots/iCub/iCub_RightHand.xml");
//std::string robFile("robots/iCub/iCub_testFinger.xml");
if (VirtualRobot::RuntimeEnvironment::hasValue("robot"))
@@ -50,6 +52,13 @@ int main(int argc,char* argv[])
dynObj->setPosition(Eigen::Vector3f(3000,3000,1000.0f));
world->addObject(dynObj);
+#if 0
+ std::string f = "/home/niko/coding/armarx/SimulationX/data/environment/KIT_Robot_Kitchen.xml";
+ ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(f);
+ SimDynamics::DynamicsObjectPtr dynObj2 = world->CreateDynamicsObject(mo,DynamicsObject::eKinematic);
+ //dynObj->setPosition(Eigen::Vector3f(3000,3000,1000.0f));
+ world->addObject(dynObj2);
+#endif
VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFile);
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robFile);
diff --git a/SimDynamics/examples/SimDynamicsViewer/SimDynamicsViewer.cpp b/SimDynamics/examples/SimDynamicsViewer/SimDynamicsViewer.cpp
index e24b2f3f0..221b99645 100644
--- a/SimDynamics/examples/SimDynamicsViewer/SimDynamicsViewer.cpp
+++ b/SimDynamics/examples/SimDynamicsViewer/SimDynamicsViewer.cpp
@@ -2,6 +2,7 @@
#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/XML/RobotIO.h>
+
#include <VirtualRobot/Visualization/VisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/RuntimeEnvironment.h>
@@ -41,6 +42,7 @@ int main(int argc, char *argv[])
//std::string filename("robots/examples/SimpleRobot/Joint6.xml");
//std::string filename("robots/ArmarIII/ArmarIII-RightArmTest6.xml");
+
if (VirtualRobot::RuntimeEnvironment::hasValue("robot"))
{
std::string robFile = VirtualRobot::RuntimeEnvironment::getValue("robot");
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsViewer.ui b/SimDynamics/examples/SimDynamicsViewer/simDynamicsViewer.ui
index e0cfbeb91..0cfd2421a 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsViewer.ui
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsViewer.ui
@@ -66,7 +66,7 @@
<rect>
<x>10</x>
<y>60</y>
- <width>161</width>
+ <width>211</width>
<height>16</height>
</rect>
</property>
@@ -79,7 +79,7 @@
<rect>
<x>10</x>
<y>80</y>
- <width>161</width>
+ <width>221</width>
<height>16</height>
</rect>
</property>
@@ -92,7 +92,7 @@
<rect>
<x>10</x>
<y>40</y>
- <width>161</width>
+ <width>221</width>
<height>16</height>
</rect>
</property>
@@ -105,7 +105,7 @@
<rect>
<x>10</x>
<y>20</y>
- <width>161</width>
+ <width>221</width>
<height>16</height>
</rect>
</property>
@@ -117,7 +117,7 @@
<property name="geometry">
<rect>
<x>10</x>
- <y>120</y>
+ <y>140</y>
<width>261</width>
<height>16</height>
</rect>
@@ -130,7 +130,7 @@
<property name="geometry">
<rect>
<x>10</x>
- <y>140</y>
+ <y>160</y>
<width>261</width>
<height>16</height>
</rect>
@@ -143,7 +143,7 @@
<property name="geometry">
<rect>
<x>10</x>
- <y>100</y>
+ <y>120</y>
<width>261</width>
<height>16</height>
</rect>
@@ -152,6 +152,19 @@
<string>GlobalPose (simox):</string>
</property>
</widget>
+ <widget class="QLabel" name="label_RNVelocityTarget">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>100</y>
+ <width>241</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Joint velocity target: 0</string>
+ </property>
+ </widget>
</widget>
<widget class="QComboBox" name="comboBoxRobotNode">
<property name="geometry">
@@ -360,7 +373,7 @@
<x>0</x>
<y>0</y>
<width>854</width>
- <height>21</height>
+ <height>25</height>
</rect>
</property>
</widget>
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
index abc05b2fc..4e032789f 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
@@ -5,7 +5,7 @@
#include <VirtualRobot/RuntimeEnvironment.h>
#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
#include <SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h>
-
+#include <VirtualRobot/XML/ObjectIO.h>
#include <QFileDialog>
#include <Eigen/Geometry>
@@ -61,6 +61,15 @@ SimDynamicsWindow::SimDynamicsWindow(std::string &sRobotFilename, Qt::WFlags fla
dynamicsObject->setPosition(Eigen::Vector3f(3000,3000,10000.0f));
dynamicsWorld->addObject(dynamicsObject);
+#if 1
+ std::string f = "/home/niko/coding/armarx/SimulationX/data/environment/KIT_Robot_Kitchen.xml";
+ ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(f);
+ mo->setSimulationType(VirtualRobot::SceneObject::Physics::eStatic);
+ dynamicsObject2 = dynamicsWorld->CreateDynamicsObject(mo);
+ //dynObj->setPosition(Eigen::Vector3f(3000,3000,1000.0f));
+ dynamicsWorld->addObject(dynamicsObject2);
+#endif
+
setupUI();
loadRobot(robotFilename);
@@ -172,7 +181,9 @@ void SimDynamicsWindow::buildVisualization()
useColModel = UI.checkBoxColModel->checkState() == Qt::Checked;
SceneObject::VisualizationType colModel = useColModel?SceneObject::Collision:SceneObject::Full;
viewer->addVisualization(dynamicsRobot,colModel);
- viewer->addVisualization(dynamicsObject,colModel);
+ viewer->addVisualization(dynamicsObject,colModel);
+ if (dynamicsObject2)
+ viewer->addVisualization(dynamicsObject2,colModel);
}
@@ -337,6 +348,7 @@ void SimDynamicsWindow::updateJointInfo()
QString qJV("Joint value: 0");
QString qTarget("Joint target: 0");
QString qVel("Joint velocity: 0");
+ QString qVelTarget("Joint velocity target: 0");
QString qGP("GlobalPose (simox): 0/0/0");
QString qVisu("VISU (simox): 0/0/0");
QString qCom("COM (bullet): 0/0/0");
@@ -389,6 +401,17 @@ void SimDynamicsWindow::updateJointInfo()
info += ",vel:";
a1 = (const char*)tmp.toAscii();
info += a1;
+
+ qVelTarget = QString("Joint velocity target: ");
+ if (dynamicsRobot->isNodeActuated(rn))
+ tmp = QString::number(dynamicsRobot->getJointTargetSpeed(rn),'f',3);
+ else
+ tmp = QString("-");
+ qVelTarget +=tmp;
+ info += ",velTarget:";
+ a1 = (const char*)tmp.toAscii();
+ info += a1;
+
Eigen::Matrix4f gp = rn->getGlobalPose();
qGP = QString("GlobalPose (simox):");
@@ -432,7 +455,8 @@ void SimDynamicsWindow::updateJointInfo()
UI.label_RNName->setText(qName);
UI.label_RNValue->setText(qJV);
UI.label_RNTarget->setText(qTarget);
- UI.label_RNVelocity->setText(qVel);
+ UI.label_RNVelocity->setText(qVel);
+ UI.label_RNVelocityTarget->setText(qVelTarget);
UI.label_RNPosGP->setText(qGP);
UI.label_RNPosVisu->setText(qVisu);
UI.label_RNPosCom->setText(qCom);
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
index cfd64cf0c..5ab718843 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
@@ -74,6 +74,7 @@ protected:
SimDynamics::DynamicsWorldPtr dynamicsWorld;
SimDynamics::DynamicsRobotPtr dynamicsRobot;
SimDynamics::DynamicsObjectPtr dynamicsObject;
+ SimDynamics::DynamicsObjectPtr dynamicsObject2;
Ui::MainWindowBulletViewer UI;
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index b0a70ad63..c979f36d2 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -506,7 +506,7 @@ Eigen::Vector3f SceneObject::getCoMGlobal()
return toGlobalCoordinateSystemVec(result);
}
-float SceneObject::getMass()
+float SceneObject::getMass() const
{
return physics.massKg;
}
@@ -765,7 +765,17 @@ std::string SceneObject::getSceneObjectXMLString(const std::string &basePath, in
void SceneObject::setMass( float m )
{
- physics.massKg = m;
+ physics.massKg = m;
+}
+
+SceneObject::Physics::SimulationType SceneObject::getSimulationType() const
+{
+ return physics.simType;
+}
+
+void SceneObject::setSimulationType(SceneObject::Physics::SimulationType s)
+{
+ physics.simType = s;
}
void SceneObject::setInertiaMatrix( const Eigen::Matrix3f &im )
diff --git a/VirtualRobot/SceneObject.h b/VirtualRobot/SceneObject.h
index 1c5c629a0..55fca1a99 100644
--- a/VirtualRobot/SceneObject.h
+++ b/VirtualRobot/SceneObject.h
@@ -58,6 +58,13 @@ public:
eCustom, //!< Not related to 3d model, the position is set by hand
eVisuBBoxCenter //!< The CoM position is automatically computed from the bounding box of the collision model
};
+ enum SimulationType
+ {
+ eStatic, // cannot move, but collide
+ eKinematic, // can be moved, but no dynamics
+ eDynamic, // full dynamic simulation
+ eUnknown // not specified
+ };
Physics()
{
@@ -65,11 +72,33 @@ public:
intertiaMatrix.setIdentity();
massKg = 0.0f;
comLocation = eVisuBBoxCenter;
+ simType = eUnknown;
}
+ std::string getString(SimulationType s) const
+ {
+ std::string r;
+ switch (s)
+ {
+ case eStatic:
+ r = "Static";
+ break;
+ case eKinematic:
+ r = "Kinematic";
+ break;
+ case eDynamic:
+ r = "Dynamic";
+ break;
+ default:
+ r = "Unknown";
+ }
+ return r;
+ }
+
void print() const
{
- std::cout << " ** Mass: ";
- if (massKg<=0)
+ std::cout << " ** Simulation Type: " << getString(simType) << endl;
+ std::cout << " ** Mass: ";
+ if (massKg<=0)
std::cout << "<not set>" << std::endl;
else
std::cout << massKg << " [kg]" << std::endl;
@@ -109,8 +138,10 @@ public:
for (int i=0;i<tabs;i++)
ta += "\t";
ss << ta << "<Physics>\n";
- ss << ta << "\t<Mass unit='kg' value='" << massKg << "'/>\n";
- ss << ta << "\t<CoM location=";
+ if (simType!=eUnknown)
+ ss << ta << "\t<SimulationType value='" << getString(simType) << "'/>\n";
+ ss << ta << "\t<Mass unit='kg' value='" << massKg << "'/>\n";
+ ss << ta << "\t<CoM location=";
if (comLocation==eVisuBBoxCenter)
ss << "'VisualizationBBoxCenter'/>\n";
else
@@ -130,6 +161,7 @@ public:
float massKg; //!< The mass of this object
CoMLocation comLocation; //!< Where is the CoM located
Eigen::Matrix3f intertiaMatrix; //! in kg*m^2
+ SimulationType simType;
std::vector< std::string > ignoreCollisions; // ignore collisions with other objects (only used within collision engines)
};
@@ -293,10 +325,16 @@ public:
/*!
Mass in Kg
*/
- float getMass();
+ float getMass() const;
void setMass(float m);
+ /*!
+ The simulation type is of interest in SimDynamics.
+ */
+ Physics::SimulationType getSimulationType() const;
+ void setSimulationType(Physics::SimulationType s);
+
/*
Inertia matrix in kg*m^2.
*/
@@ -390,7 +428,7 @@ protected:
virtual void updatePose( const Eigen::Matrix4f &parentPose, bool updateChildren = true );
- SceneObject(){};
+ SceneObject(){}
//! basic data, used by Obstacle and ManipulationObject
std::string getSceneObjectXMLString(const std::string &basePath, int tabs);
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index b6952c163..e3601ac82 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -632,7 +632,7 @@ namespace VirtualRobot {
return node;
}
- SoSeparator* CoinVisualizationFactory::CreatePlaneVisualization( const Eigen::Vector3f &position, const Eigen::Vector3f &normal, float extend, float transparency, bool grid, float colorR /*= 0.5f*/, float colorG /*= 0.5f*/, float colorB /*= 0.5f*/ )
+ SoSeparator* CoinVisualizationFactory::CreatePlaneVisualization( const Eigen::Vector3f &position, const Eigen::Vector3f &normal, float extend, float transparency, bool grid, float colorR /*= 0.5f*/, float colorG /*= 0.5f*/, float colorB /*= 0.5f*/, std::string textureFile )
{
SoSeparator *res = new SoSeparator();
res->ref();
@@ -658,16 +658,22 @@ namespace VirtualRobot {
if (grid)
{
SoSeparator *res2;
- if (transparency==0)
+ if (!textureFile.empty() && RuntimeEnvironment::getDataFileAbsolute(textureFile))
{
- std::string filename("images/FloorWhite.png");
- RuntimeEnvironment::getDataFileAbsolute(filename);
- res2 = CreateGrid(extend,extend,extend/500.0f,extend/500.0f,true,filename.c_str(),transparency);
+ res2 = CreateGrid(extend,extend,extend/500.0f,extend/500.0f,true,textureFile.c_str(),transparency);
} else
{
- std::string filename("images/Floor.png");
- RuntimeEnvironment::getDataFileAbsolute(filename);
- res2 = CreateGrid(extend,extend,extend/500.0f,extend/500.0f,true,filename.c_str(),transparency);
+ if (transparency==0)
+ {
+ std::string filename("images/FloorWhite.png");
+ RuntimeEnvironment::getDataFileAbsolute(filename);
+ res2 = CreateGrid(extend,extend,extend/500.0f,extend/500.0f,true,filename.c_str(),transparency);
+ } else
+ {
+ std::string filename("images/Floor.png");
+ RuntimeEnvironment::getDataFileAbsolute(filename);
+ res2 = CreateGrid(extend,extend,extend/500.0f,extend/500.0f,true,filename.c_str(),transparency);
+ }
}
res->addChild(res2);
} else
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
index 3fdec13ec..89bc44464 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
@@ -104,7 +104,7 @@ public:
static SoSeparator* CreateConvexHull2DVisualization(const MathTools::ConvexHull2DPtr ch, MathTools::Plane &p, VisualizationFactory::Color colorInner = VisualizationFactory::Color::Blue(), VisualizationFactory::Color colorLine = VisualizationFactory::Color::Black(), float lineSize = 5.0f, const Eigen::Vector3f &offset =Eigen::Vector3f::Zero() );
static SoSeparator* CreatePolygonVisualization(const std::vector<Eigen::Vector3f> &points, VisualizationFactory::Color colorInner = VisualizationFactory::Color::Blue(), VisualizationFactory::Color colorLine = VisualizationFactory::Color::Black(), float lineSize = 4.0f);
- static SoSeparator* CreatePlaneVisualization(const Eigen::Vector3f &position, const Eigen::Vector3f &normal, float extend, float transparency, bool grid=true, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f);
+ static SoSeparator* CreatePlaneVisualization(const Eigen::Vector3f &position, const Eigen::Vector3f &normal, float extend, float transparency, bool grid=true, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f, std::string textureFile = std::string());
static SoSeparator* CreateCoordSystemVisualization(float scaling = 1.0f, std::string *text = NULL, float axisLength = 100.0f, float axisSize = 3.0f, int nrOfBlocks = 10);
static SoSeparator* CreateBoundingBox(SoNode* ivModel, bool wireFrame=false);
static SoSeparator* CreateGrid(float width,float depth,float widthMosaic,float depthMosaic,bool InvertNormal,const char* pFileName,float Transparency);
diff --git a/VirtualRobot/XML/BaseIO.cpp b/VirtualRobot/XML/BaseIO.cpp
index 261fb843e..7d9abaa97 100644
--- a/VirtualRobot/XML/BaseIO.cpp
+++ b/VirtualRobot/XML/BaseIO.cpp
@@ -937,6 +937,21 @@ void BaseIO::processPhysicsTag(rapidxml::xml_node<char> *physicsXMLNode, const s
physics.ignoreCollisions.push_back(s);
ignoreColXMLNode = ignoreColXMLNode->next_sibling("ignorecollision",0,false);
}
+ rapidxml::xml_node<> *simulationtype = physicsXMLNode->first_node("simulationtype",0,false);
+ if (simulationtype)
+ {
+ rapidxml::xml_attribute<> *attr = simulationtype->first_attribute("value", 0, false);
+ THROW_VR_EXCEPTION_IF(!attr, "Expecting 'value' attribute in <SimulationType> tag..." << endl)
+ std::string s(attr->value());
+ getLowerCase(s);
+ if (s=="dynamic" || s=="dynamics")
+ physics.simType = VirtualRobot::SceneObject::Physics::eDynamic;
+ else if (s=="static")
+ physics.simType = VirtualRobot::SceneObject::Physics::eStatic;
+ else if (s=="kinematic")
+ physics.simType = VirtualRobot::SceneObject::Physics::eKinematic;
+ // otherwise eUnknown remains
+ }
}
std::string BaseIO::processFileNode( rapidxml::xml_node<char> *fileNode, const std::string &basePath )
diff --git a/VirtualRobot/XML/ObjectIO.cpp b/VirtualRobot/XML/ObjectIO.cpp
index 8fc600c42..0a74a90ff 100644
--- a/VirtualRobot/XML/ObjectIO.cpp
+++ b/VirtualRobot/XML/ObjectIO.cpp
@@ -278,9 +278,32 @@ ObstaclePtr ObjectIO::processObstacle(rapidxml::xml_node<char>* objectXMLNode, c
bool physicsDefined = false;
Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
- // get name
- std::string objName = processNameAttribute(objectXMLNode);
- THROW_VR_EXCEPTION_IF (objName.empty(),"Obstacle definition expects attribute 'name'");
+ // get name
+ std::string objName = processNameAttribute(objectXMLNode);
+
+ // first check if there is an xml file to load
+ rapidxml::xml_node<>* xmlFileNode = objectXMLNode->first_node("file",0,false);
+ if (xmlFileNode)
+ {
+ std::string xmlFile = processFileNode(xmlFileNode,basePath);
+ ObstaclePtr result = loadObstacle(xmlFile);
+ if (!result)
+ return result;
+ if (!objName.empty())
+ result->setName(objName);
+ // update global pose
+ rapidxml::xml_node<>* poseNode = objectXMLNode->first_node("globalpose",0,false);
+ if (poseNode)
+ {
+ processTransformNode(poseNode, objName, globalPose);
+ result->setGlobalPose(globalPose);
+ }
+
+ return result;
+ }
+
+ THROW_VR_EXCEPTION_IF (objName.empty(),"Obstacle definition expects attribute 'name'");
+
rapidxml::xml_node<>* node = objectXMLNode->first_node();
while (node)
--
GitLab