diff --git a/CMakeLists.txt b/CMakeLists.txt
index 927cb12b2ba83a1312e26919f94e11559fc36eab..7f4939011fc20b74729f206b17f5143d4291eb6f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,14 +3,25 @@ PROJECT(Simox)
cmake_minimum_required(VERSION 2.6)
MESSAGE(STATUS "******************** Configuring Simox ************************")
set (Simox_BUILD_DIRECTORY ${CMAKE_BINARY_DIR} CACHE STRING "Simox build directory")
+#set (Simox_BUILD_VirtualRobot TRUE CACHE BOOL "Build Simox VirtualRobot")
+set (Simox_BUILD_Saba TRUE CACHE BOOL "Build Motion Planning library")
+set (Simox_BUILD_GraspStudio TRUE CACHE BOOL "Build Grasp Planning library")
+set (Simox_BUILD_SimDynamics FALSE CACHE BOOL "Build Dynamic Simulation")
INCLUDE (config.cmake)
MESSAGE(STATUS "\n** SETTING Simox target directory to: ${Simox_BUILD_DIRECTORY}")
add_subdirectory(VirtualRobot)
-add_subdirectory(MotionPlanning)
-add_subdirectory(GraspPlanning)
+if (Simox_BUILD_Saba)
+ add_subdirectory(MotionPlanning)
+endif()
+if (Simox_BUILD_GraspStudio)
+ add_subdirectory(GraspPlanning)
+endif()
+if (Simox_BUILD_SimDynamics)
+ add_subdirectory(SimDynamics)
+endif()
add_subdirectory(doc)
diff --git a/GraspPlanning/GraspPlanner/GenericGraspPlanner.h b/GraspPlanning/GraspPlanner/GenericGraspPlanner.h
index 72d4b6ba19ef62512c76bbfca7bd7e4631dce755..03f6162d3707ad901b049dc77ca14c3275780ff8 100644
--- a/GraspPlanning/GraspPlanner/GenericGraspPlanner.h
+++ b/GraspPlanning/GraspPlanner/GenericGraspPlanner.h
@@ -23,7 +23,7 @@
#ifndef __GENERIC_GRASP_PLANNER_H__
#define __GENERIC_GRASP_PLANNER_H__
-#include "GraspStudio.h"
+#include "../GraspStudio.h"
#include "GraspPlanner.h"
#include "ApproachMovementGenerator.h"
#include "GraspQuality/GraspQualityMeasure.h"
diff --git a/GraspPlanning/GraspPlanner/GraspPlanner.h b/GraspPlanning/GraspPlanner/GraspPlanner.h
index 526db3f9423cd77098cc94e3216f7e20ad4488ab..322390c14b8979b76bd10c8a681ad6610c0f6404 100644
--- a/GraspPlanning/GraspPlanner/GraspPlanner.h
+++ b/GraspPlanning/GraspPlanner/GraspPlanner.h
@@ -23,7 +23,7 @@
#ifndef __GENERAL_GRASP_PLANNER_H__
#define __GENERAL_GRASP_PLANNER_H__
-#include "GraspStudio.h"
+#include "../GraspStudio.h"
#include <VirtualRobot/EndEffector/EndEffector.h>
#include <VirtualRobot/SceneObject.h>
#include <VirtualRobot/Grasping/GraspSet.h>
diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlanner.cpp b/GraspPlanning/examples/GraspPlanner/GraspPlanner.cpp
index fb8ac8fe6487a3308ed8ca50de6eedd43f006c09..8671f6e3e75967a8651d2e975d2553b9459bbf5e 100644
--- a/GraspPlanning/examples/GraspPlanner/GraspPlanner.cpp
+++ b/GraspPlanning/examples/GraspPlanner/GraspPlanner.cpp
@@ -23,9 +23,10 @@ int main(int argc, char *argv[])
// --robot robots/iCub/iCub.xml --endeffector "Left Hand" --preshape "Grasp Preshape"
std::string robot("robots/ArmarIII/ArmarIII.xml");
VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robot);
- std::string eef("Hand L");
+ std::string eef("Hand R");
//std::string object("objects/wok.xml");
- std::string object("objects/riceBox.xml");
+ //std::string object("objects/riceBox.xml");
+ std::string object("objects/WaterBottleSmall.xml");
VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(object);
std::string preshape("");
diff --git a/MotionPlanning/CSpace/CSpace.cpp b/MotionPlanning/CSpace/CSpace.cpp
index 8962653512b7f9605705949d250cbe0dffe37d7b..547f15c881eca29d069e2834e16436e3c1eeade5 100644
--- a/MotionPlanning/CSpace/CSpace.cpp
+++ b/MotionPlanning/CSpace/CSpace.cpp
@@ -349,8 +349,7 @@ float CSpace::calculateObstacleDistance(const Eigen::VectorXf &config)
colCheckMutex.lock();
// set configuration (joint values)
-
- robotNodes->setJointValues(config);
+ robo->setJointValues(robotNodes,config);
float d = cdm->getDistance();
if (multiThreaded)
@@ -396,7 +395,7 @@ bool CSpace::isCollisionFree( const Eigen::VectorXf &config )
colCheckMutex.lock();
// set configuration (joint values)
- robotNodes->setJointValues(config);
+ robo->setJointValues(robotNodes,config);
bool res = cdm->isInCollision();
if (multiThreaded)
diff --git a/MotionPlanning/Planner/GraspRrt.cpp b/MotionPlanning/Planner/GraspRrt.cpp
index aa911c87e9f7b5b80810ff1efecd29a37b82b793..6c881b5d15f53dbfadbb9262fa89f02a3b08769b 100644
--- a/MotionPlanning/Planner/GraspRrt.cpp
+++ b/MotionPlanning/Planner/GraspRrt.cpp
@@ -29,8 +29,9 @@ GraspRrt::GraspRrt( CSpaceSampledPtr cspace,
{
plannerInitialized = false;
name ="GraspRrt";
- THROW_VR_EXCEPTION_IF(!cspace || !object || !eef || !cspace->getRobotNodeSet() || !measure, "NULL data");
+ THROW_VR_EXCEPTION_IF(!cspace || !object || !eef || !cspace->getRobotNodeSet() || !cspace->getRobot() || !measure, "NULL data");
rns = cspace->getRobotNodeSet();
+ robot = cspace->getRobot();
targetObject = object;
this->eef = eef;
this->graspQualityMeasure = measure;
@@ -362,8 +363,9 @@ bool GraspRrt::calculateGlobalGraspPose(const Eigen::VectorXf &c, Eigen::Matrix4
int nId1,nId2;
Eigen::Matrix4f mMat,mRotMat;
+
// set gcp object
- rns->setJointValues(c);
+ robot->setJointValues(rns,c);
gcpOject->setGlobalPose(eef->getGCP()->getGlobalPose());
// get target position (position on grasp object with shortest distance to hand)
@@ -691,7 +693,7 @@ GraspRrt::MoveArmResult GraspRrt::createWorkSpaceSamplingStep(const Eigen::Matri
}
MathHelpers::deltaQuat(&(pExtendNode->cartPosTCP1[3]),&(pCartGoalPose[3]),&(pDeltaPosQuat_Global[3]));
*/
- rns->setJointValues(extendNode->configuration);
+ robot->setJointValues(rns,extendNode->configuration);
Eigen::Matrix4f currentPose = mapConfigTcp[extendNode];
return createWorkSpaceSamplingStep (currentPose,goalPose,storeCSpaceConf);
}
@@ -775,7 +777,7 @@ float GraspRrt::calculateGraspScore(const Eigen::VectorXf &c, int nId, bool bSto
clock_t timeStart = clock();
performanceMeasure.numberOfGraspScorings++;
- rns->setJointValues(c);
+ robot->setJointValues(rns,c);
VirtualRobot::EndEffector::ContactInfoVector contactsAll = eef->closeActors(graspCollisionObjects);
VirtualRobot::EndEffector::ContactInfoVector contacts;
// we only need the targetObject contacts
@@ -981,7 +983,7 @@ bool GraspRrt::processNode( CSpaceNodePtr n )
if (!n)
return false;
// get tcp pose
- rns->setJointValues(n->configuration);
+ robot->setJointValues(rns,n->configuration);
// using gcp!
Eigen::Matrix4f p = eef->getGCP()->getGlobalPose();
diff --git a/MotionPlanning/Planner/GraspRrt.h b/MotionPlanning/Planner/GraspRrt.h
index d931357419cb8659d9adfae3012de46870f6b7cc..f1c8067419d3084ac24013bbbd763a9b712c7761 100644
--- a/MotionPlanning/Planner/GraspRrt.h
+++ b/MotionPlanning/Planner/GraspRrt.h
@@ -189,6 +189,7 @@ protected:
VirtualRobot::ObstaclePtr targetObject;
VirtualRobot::RobotNodeSetPtr rns;
VirtualRobot::EndEffectorPtr eef;
+ VirtualRobot::RobotPtr robot;
std::map < VirtualRobot::GraspPtr, Saba::CSpaceNodePtr > graspNodeMapping;
std::vector< Eigen::VectorXf > ikSolutions;
diff --git a/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp b/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp
index f9dbed8829e5543bf34ed69dd0071e8e27d7d3bb..47b67c95822b41a7111cddad65b26a6c442f874e 100644
--- a/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp
+++ b/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp
@@ -70,7 +70,7 @@ SoSeparator* CoinRrtWorkspaceVisualization::getCoinVisualization()
bool CoinRrtWorkspaceVisualization::addCSpacePath(CSpacePathPtr path, CoinRrtWorkspaceVisualization::ColorSet colorSet)
{
- if (!path || !robotNodeSet || !TCPNode)
+ if (!path || !robotNodeSet || !TCPNode || !robot)
return false;
if (path->getDimension() != robotNodeSet->getSize())
{
@@ -117,7 +117,7 @@ bool CoinRrtWorkspaceVisualization::addCSpacePath(CSpacePathPtr path, CoinRrtWor
CSpace::lock();
// get tcp coords:
- robotNodeSet->setJointValues(actConfig);
+ robot->setJointValues(robotNodeSet,actConfig);
Eigen::Matrix4f m;
m = TCPNode->getGlobalPose();
x = m(0,3);
@@ -234,7 +234,7 @@ bool CoinRrtWorkspaceVisualization::addTree(CSpaceTreePtr tree, CoinRrtWorkspace
actualNode = nodes[i];
// get tcp coords:
- robotNodeSet->setJointValues(actualNode->configuration);
+ robot->setJointValues(robotNodeSet,actualNode->configuration);
Eigen::Matrix4f m;
m = TCPNode->getGlobalPose();
p(0) = m(0,3);
@@ -344,7 +344,7 @@ bool CoinRrtWorkspaceVisualization::addConfiguration( const Eigen::VectorXf &c,
SoTranslation *t = new SoTranslation();
// get tcp coords:
- robotNodeSet->setJointValues(c);
+ robot->setJointValues(robotNodeSet,c);
Eigen::Matrix4f m;
m = TCPNode->getGlobalPose();
x = m(0,3);//[0][3];
diff --git a/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp
index a1d2d660c097f6b4b006699e57da31354b047bab..93183957c4dc1a6df373f9c1822614aea4c7df75 100644
--- a/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp
+++ b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp
@@ -491,8 +491,8 @@ void GraspRrtWindow::selectStart(int nr)
//if (robotStart)
// configs[nr]->applyToRobot(robotStart);
if (robot)
- configs[nr]->applyToRobot(robot);
- configs[nr]->setJointValues();
+ robot->setJointValues(configs[nr]);
+ //configs[nr]->setJointValues();
if (rns)
rns->getJointValues(startConfig);
}
@@ -650,7 +650,7 @@ void GraspRrtWindow::testGraspPose()
// move towards object
Eigen::Matrix4f p = eef->getGCP()->getGlobalPose();
test_graspRrt->createWorkSpaceSamplingStep(p,globalGrasp,c);
- rns->setJointValues(c);
+ robot->setJointValues(rns,c);
// test
/*Eigen::Matrix4f p1;
@@ -740,7 +740,7 @@ void GraspRrtWindow::sliderSolution( int pos )
float p = (float)pos/1000.0f;
Eigen::VectorXf iPos;
s->interpolate(p,iPos);
- rns->setJointValues(iPos);
+ robot->setJointValues(rns,iPos);
redraw();
}
diff --git a/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp b/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp
index e8ff247a45dede4e34dc0d8bbce46316f7d63e31..882fa8fd7f932644f6c2c68ad9cac85cab7f23bb 100644
--- a/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp
+++ b/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp
@@ -177,8 +177,8 @@ QString IKRRTWindow::formatString(const char *s, float f)
void IKRRTWindow::resetSceneryAll()
{
- if (rns)
- rns->setJointValues(startConfig);
+ if (rns && robot)
+ robot->setJointValues(rns, startConfig);
}
@@ -610,7 +610,7 @@ void IKRRTWindow::sliderSolution( int pos )
float p = (float)pos/1000.0f;
Eigen::VectorXf iPos;
s->interpolate(p,iPos);
- rns->setJointValues(iPos);
+ robot->setJointValues(rns,iPos);
m_pExViewer->scheduleRedraw();
}
diff --git a/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningScenery.cpp b/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningScenery.cpp
index 41d8cf0058fb5f5e6f4c242c43b3f47fe4311ee5..448ed94fe314e46d08a89ecbb18b8f5f27fb9654 100644
--- a/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningScenery.cpp
+++ b/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningScenery.cpp
@@ -294,7 +294,7 @@ void MTPlanningScenery::buildPlanningThread(bool bMultiCollisionCheckers, int id
start[1] = y;
start[2] = z;
cout << "START: " << x << "," << y << "," << z << endl;
- kinChain->setJointValues(start);
+ pRobot->setJointValues(kinChain,start);
}while(pCcm->isInCollision());
startPositions.push_back(start);
@@ -304,7 +304,7 @@ void MTPlanningScenery::buildPlanningThread(bool bMultiCollisionCheckers, int id
goal[1] = y;
goal[2] = z;
cout << "GOAL: " << x << "," << y << "," << z << endl;
- kinChain->setJointValues(goal);
+ pRobot->setJointValues(kinChain,goal);
}while(pCcm->isInCollision());
@@ -340,10 +340,10 @@ void MTPlanningScenery::buildPlanningThread(bool bMultiCollisionCheckers, int id
RobotNodePtr rn = pRobot->getRobotNode(TCPName);
if (!rn)
return;
- kinChain->setJointValues(start);
+ pRobot->setJointValues(kinChain,start);
Eigen::Matrix4f gp = rn->getGlobalPose();
SoMatrixTransform *mt = CoinVisualizationFactory::getMatrixTransform(gp);
- kinChain->setJointValues(goal);
+ pRobot->setJointValues(kinChain,goal);
gp = rn->getGlobalPose();
SoMatrixTransform *mt2 =CoinVisualizationFactory::getMatrixTransform(gp);
diff --git a/MotionPlanning/examples/RRT/RRTdemo.cpp b/MotionPlanning/examples/RRT/RRTdemo.cpp
index 877e8bff936aca60cae367b5b66b81627d81f535..a57d4dc10745e630ca1ddf3c695d83e264129e68 100644
--- a/MotionPlanning/examples/RRT/RRTdemo.cpp
+++ b/MotionPlanning/examples/RRT/RRTdemo.cpp
@@ -157,7 +157,7 @@ void startRRTVisualization()
- planningNodes->setJointValues(start);
+ robot->setJointValues(planningNodes,start);
// display robot
SoSeparator *sep = new SoSeparator();
diff --git a/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp b/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp
index aebb2a004b1bad7778805b67129773039cd75ca9..aab72b88e5dbe6bb90c65316ca0e148543282cfb 100644
--- a/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp
+++ b/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp
@@ -392,10 +392,10 @@ void RrtGuiWindow::selectStart(int nr)
if (nr<0 || nr>=(int)configs.size())
return;
if (robotStart)
- configs[nr]->applyToRobot(robotStart);
+ robotStart->setJointValues(configs[nr]);
if (robot)
- configs[nr]->applyToRobot(robot);
- configs[nr]->setJointValues();
+ robot->setJointValues(configs[nr]);
+ //configs[nr]->setJointValues();
if (rns)
rns->getJointValues(startConfig);
}
@@ -405,8 +405,8 @@ void RrtGuiWindow::selectGoal(int nr)
if (nr<0 || nr>=(int)configs.size())
return;
if (robotGoal)
- configs[nr]->applyToRobot(robotGoal);
- configs[nr]->setJointValues();
+ robotGoal->setJointValues(configs[nr]);
+ robot->setJointValues(configs[nr]);
if (rns)
rns->getJointValues(goalConfig);
}
@@ -573,7 +573,7 @@ void RrtGuiWindow::sliderSolution( int pos )
float p = (float)pos/1000.0f;
Eigen::VectorXf iPos;
s->interpolate(p,iPos);
- rns->setJointValues(iPos);
+ robot->setJointValues(rns,iPos);
redraw();
}
diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 6154826aefafc152b75185c619082977dfe52451..b5f0739de42de713e12c910791ef3fde79210496 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -23,6 +23,7 @@ Nodes/RobotNodeRevolute.cpp
Nodes/RobotNodeRevoluteFactory.cpp
Nodes/RobotNodeFixed.cpp
Nodes/RobotNodeFixedFactory.cpp
+Nodes/RobotNodeActuator.cpp
Transformation/Transformation.cpp
Visualization/Visualization.cpp
Visualization/VisualizationNode.cpp
@@ -82,6 +83,8 @@ Nodes/RobotNodeRevolute.h
Nodes/RobotNodeRevoluteFactory.h
Nodes/RobotNodeFixed.h
Nodes/RobotNodeFixedFactory.h
+Nodes/RobotNodeActuator.h
+Nodes/ConditionedLock.h
Transformation/DHParameter.h
Transformation/Transformation.h
Visualization/VisualizationFactory.h
diff --git a/VirtualRobot/Compression/tests/CompressionBZip2Test.cpp b/VirtualRobot/Compression/tests/CompressionBZip2Test.cpp
index c2530d4faa7f5419e7b532eb400b2b5fba56c346..bdb60d1e4255a640ff6583caada02f30d24dfc0c 100644
--- a/VirtualRobot/Compression/tests/CompressionBZip2Test.cpp
+++ b/VirtualRobot/Compression/tests/CompressionBZip2Test.cpp
@@ -13,6 +13,7 @@
#include <fstream>
#include <istream>
+#include <time.h>
BOOST_AUTO_TEST_SUITE(Compression)
diff --git a/VirtualRobot/EndEffector/EndEffector.cpp b/VirtualRobot/EndEffector/EndEffector.cpp
index 2fe0c1cca5606860f8212d2150d21f03bf860828..dcbcfcf1eeed52023fa5b19e10e5239db289f252 100644
--- a/VirtualRobot/EndEffector/EndEffector.cpp
+++ b/VirtualRobot/EndEffector/EndEffector.cpp
@@ -81,7 +81,7 @@ EndEffectorPtr EndEffector::clone(RobotPtr newRobot)
// set current config to new eef
RobotConfigPtr currentConfig = getConfiguration();
RobotConfigPtr newConfig = currentConfig->clone(newRobot);
- newConfig->setJointValues();
+ newRobot->setJointValues(newConfig);
return eef;
}
@@ -288,7 +288,9 @@ bool EndEffector::setPreshape( const std::string &name )
RobotConfigPtr ps = getPreshape(name);
if (ps)
{
- ps->setJointValues();
+ RobotPtr robot = getRobot();
+ VR_ASSERT(robot);
+ robot->setJointValues(ps);
return true;
}
return false;
diff --git a/VirtualRobot/EndEffector/EndEffectorActor.cpp b/VirtualRobot/EndEffector/EndEffectorActor.cpp
index 9bfd31bf13deec96d97c927ca8c5e8411a866b0e..d24db4821cfe836074ef79778b3581d369fe3a93 100644
--- a/VirtualRobot/EndEffector/EndEffectorActor.cpp
+++ b/VirtualRobot/EndEffector/EndEffectorActor.cpp
@@ -52,13 +52,18 @@ std::string EndEffectorActor::getName()
bool EndEffectorActor::moveActor(float angle)
{
+ if (actors.size()==0)
+ return true;
+ RobotPtr robot = actors[0].robotNode->getRobot();
+ VR_ASSERT(robot);
bool res = true;
for(std::vector<ActorDefinition>::iterator n = actors.begin(); n != actors.end(); n++)
{
float v = n->robotNode->getJointValue() + angle * n->directionAndSpeed;
if(v <= n->robotNode->getJointLimitHi() && v >= n->robotNode->getJointLimitLo())
{
- n->robotNode->setJointValue(v);
+ robot->setJointValue(n->robotNode,v);
+ //n->robotNode->setJointValue(v);
res = false;
}
}
@@ -67,10 +72,9 @@ bool EndEffectorActor::moveActor(float angle)
bool EndEffectorActor::moveActorCheckCollision( EndEffectorPtr eef, EndEffector::ContactInfoVector &storeContacts, SceneObjectSetPtr obstacles /*= SceneObjectSetPtr()*/, float angle /*= 0.02*/ )
{
- if (!eef)
- {
- THROW_VR_EXCEPTION ("NULL eef...");
- }
+ VR_ASSERT(eef);
+ RobotPtr robot = eef->getRobot();
+ VR_ASSERT(robot);
bool res = true;
std::vector<EndEffectorActorPtr> eefActors;
eef->getActors(eefActors);
@@ -84,7 +88,8 @@ bool EndEffectorActor::moveActorCheckCollision( EndEffectorPtr eef, EndEffector:
float v = oldV + angle * n->directionAndSpeed;
if(v <= n->robotNode->getJointLimitHi() && v >= n->robotNode->getJointLimitLo())
{
- n->robotNode->setJointValue(v);
+ robot->setJointValue(n->robotNode,v);
+ //n->robotNode->setJointValue(v);
// check collision
bool collision = false;
@@ -121,7 +126,8 @@ bool EndEffectorActor::moveActorCheckCollision( EndEffectorPtr eef, EndEffector:
else
{
// reset last position
- n->robotNode->setJointValue(oldV);
+ //n->robotNode->setJointValue(oldV);
+ robot->setJointValue(n->robotNode,oldV);
}
}
}
@@ -154,7 +160,7 @@ bool EndEffectorActor::moveActorCheckCollision( EndEffectorPtr eef, EndEffector:
Eigen::Vector3f contGlobal2 = newContacts[i].robotNode->toGlobalCoordinateSystemVec(contFinger);
newContacts[i].approachDirectionGlobal = contGlobal2 - contGlobal1;
newContacts[i].approachDirectionGlobal.normalize();
- config->setJointValues();
+ robot->setJointValues(config);
storeContacts.push_back(newContacts[i]);
}
diff --git a/VirtualRobot/IK/CoMIK.cpp b/VirtualRobot/IK/CoMIK.cpp
index 45ce937e648162db59adaaf833917df2ffbd6b5d..80b079c6783af72284123c60342fbaf46802fe37 100644
--- a/VirtualRobot/IK/CoMIK.cpp
+++ b/VirtualRobot/IK/CoMIK.cpp
@@ -3,6 +3,7 @@
#include "../Nodes/RobotNodeRevolute.h"
#include "../Nodes/RobotNodePrismatic.h"
#include "../VirtualRobotException.h"
+#include "../Robot.h"
#include <float.h>
@@ -158,6 +159,8 @@ bool CoMIK::isValid(const Eigen::VectorXf &v) const
bool CoMIK::computeSteps(float stepSize, float minumChange, int maxNStep)
{
std::vector<RobotNodePtr> rn = m_RobotNodeSet->getAllRobotNodes();
+ RobotPtr robot = m_RobotNodeSet->getRobot();
+ std::vector<float> jv(m_RobotNodeSet->getSize(),0.0f);
int step = 0;
checkTolerances();
float lastDist = FLT_MAX;
@@ -174,8 +177,9 @@ bool CoMIK::computeSteps(float stepSize, float minumChange, int maxNStep)
}
for (unsigned int i=0; i<rn.size();i++)
- rn[i]->setJointValue(rn[i]->getJointValue() + dTheta[i]);
-
+ jv[i] = (rn[i]->getJointValue() + dTheta[i]);
+ //rn[i]->setJointValue(rn[i]->getJointValue() + dTheta[i]);
+ robot->setJointValues(m_RobotNodeSet,jv);
// check tolerances
if (checkTolerances())
{
diff --git a/VirtualRobot/IK/DifferentialIK.cpp b/VirtualRobot/IK/DifferentialIK.cpp
index e9e52f4d239a415223fae72a58c44d4f0ae339ea..f2eb851825068efac8e8f675227fb234aa061888 100644
--- a/VirtualRobot/IK/DifferentialIK.cpp
+++ b/VirtualRobot/IK/DifferentialIK.cpp
@@ -321,7 +321,12 @@ void DifferentialIK::checkImprovements( bool enable )
bool DifferentialIK::computeSteps(float stepSize, float minumChange, int maxNStep)
{
+ VR_ASSERT(rns);
+ VR_ASSERT(nodes.size() == rns->getSize());
//std::vector<RobotNodePtr> rn = this->nodes;
+ RobotPtr robot = rns->getRobot();
+ VR_ASSERT(robot);
+ std::vector<float> jv(nodes.size(),0.0f);
int step = 0;
checkTolerances();
float lastDist = FLT_MAX;
@@ -329,10 +334,14 @@ bool DifferentialIK::computeSteps(float stepSize, float minumChange, int maxNSte
while (step<maxNStep)
{
VectorXf dTheta = this->computeStep(stepSize);
+
for (unsigned int i=0; i<nodes.size();i++)
{
- nodes[i]->setJointValue(nodes[i]->getJointValue() + dTheta[i]);
+ jv[i] = (nodes[i]->getJointValue() + dTheta[i]);
+ //nodes[i]->setJointValue(nodes[i]->getJointValue() + dTheta[i]);
}
+ robot->setJointValues(rns,jv);
+
// check tolerances
if (checkTolerances())
{
diff --git a/VirtualRobot/IK/GenericIKSolver.cpp b/VirtualRobot/IK/GenericIKSolver.cpp
index e9153412f9c95939225f079e466ca94cf228e858..6215f03196d472e35e5592632a330fc1bc1d9511 100644
--- a/VirtualRobot/IK/GenericIKSolver.cpp
+++ b/VirtualRobot/IK/GenericIKSolver.cpp
@@ -68,7 +68,8 @@ void GenericIKSolver::setJointsRandom()
float v = ro->getJointLimitLo() + (ro->getJointLimitHi() - ro->getJointLimitLo()) * r;
jv.push_back(v);
}
- rns->setJointValues(jv);
+ RobotPtr rob = rns->getRobot();
+ rob->setJointValues(rns,jv);
}
bool GenericIKSolver::trySolve()
diff --git a/VirtualRobot/IK/IKSolver.cpp b/VirtualRobot/IK/IKSolver.cpp
index a765d4f89a50f2c45ccc5b59c072a389e31406b8..824fd12302f138ccbdbac48102619d51ac952ad7 100644
--- a/VirtualRobot/IK/IKSolver.cpp
+++ b/VirtualRobot/IK/IKSolver.cpp
@@ -78,8 +78,8 @@ std::vector<float> IKSolver::solveNoRNSUpdate(const Eigen::Matrix4f &globalPose,
{
rns->getJointValues(result);
}
-
- rns->setJointValues(v);
+ RobotPtr rob = rns->getRobot();
+ rob->setJointValues(rns,v);
return result;
}
@@ -166,7 +166,7 @@ bool IKSolver::solve( ManipulationObjectPtr object, GraspPtr grasp, CartesianSel
robot->applyJointValues();
return true;
}
- rns->setJointValues(v);
+ robot->setJointValues(rns,v);
robot->setUpdateVisualization(updateStatus);
return false;
}
@@ -196,7 +196,8 @@ GraspPtr IKSolver::sampleSolution( ManipulationObjectPtr object, GraspSetPtr gra
return g;
// did not succeed, reset joint values and remove grasp from temporary set
- rns->setJointValues(v);
+ RobotPtr rob = rns->getRobot();
+ rob->setJointValues(rns,v);
if (removeGraspFromSet)
graspSet->removeGrasp(g);
return GraspPtr();
diff --git a/VirtualRobot/Nodes/ConditionedLock.h b/VirtualRobot/Nodes/ConditionedLock.h
index 4895452b2b37fe9afb92351674f4fd78624029c1..c86e2e70437a46bd4593a87a5e50a0b5d97dfdf0 100755
--- a/VirtualRobot/Nodes/ConditionedLock.h
+++ b/VirtualRobot/Nodes/ConditionedLock.h
@@ -23,5 +23,6 @@ typedef ConditionedLock<boost::unique_lock<boost::recursive_mutex> > ReadLock;
typedef ConditionedLock<boost::unique_lock<boost::recursive_mutex> > WriteLock;
//typedef ConditionedLock<boost::shared_lock<boost::shared_mutex> > ReadLock;
//typedef ConditionedLock<boost::unique_lock<boost::shared_mutex> > WriteLock;
+typedef boost::shared_ptr< ReadLock > ReadLockPtr;
#endif
diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index de220531d3cfd3f2b1bd6239eaa4bd3ff1b3c395..beea3a281b98b0a6f6b716594bb6816c3fe47d47 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -14,8 +14,6 @@
#include <Eigen/Core>
-#include "ConditionedLock.h"
-
namespace VirtualRobot {
RobotNode::RobotNode( RobotWeakPtr rob,
@@ -28,8 +26,7 @@ RobotNode::RobotNode( RobotWeakPtr rob,
float jointValueOffset,
const SceneObject::Physics &p,
CollisionCheckerPtr colChecker)
- : SceneObject(name,visualization,collisionModel,p,colChecker),
- use_mutex(true)
+ : SceneObject(name,visualization,collisionModel,p,colChecker)
{
@@ -53,8 +50,6 @@ RobotNode::RobotNode( RobotWeakPtr rob,
RobotNode::~RobotNode()
{
- reset();
-
// not needed here
// when robot is destroyed all references to this RobotNode are also destroyed
//RobotPtr rob = robot.lock();
@@ -62,23 +57,6 @@ RobotNode::~RobotNode()
// rob->deregisterRobotNode(shared_from_this());
}
-void RobotNode::reset()
-{
- SceneObject::reset();
- childrenNames.clear();
- jointValueOffset = 0.0f;
- jointLimitLo = 0.0f;
- jointLimitHi = (float)M_PI;
- jointValueOffset = 0.0f;
- preJointTransformation = Eigen::Matrix4f::Identity();
- postJointTransformation = Eigen::Matrix4f::Identity();
- optionalDHParameter.isSet = false;
- globalPosePostJoint = Eigen::Matrix4f::Identity();
- jointValue = 0.0f;
- children.clear();
- parent.reset();
- initialized = false;
-}
bool RobotNode::initialize(RobotNodePtr parent, bool initializeChildren)
{
@@ -127,16 +105,15 @@ bool RobotNode::initialize(RobotNodePtr parent, bool initializeChildren)
}
-RobotPtr RobotNode::getRobot()
+RobotPtr RobotNode::getRobot() const
{
RobotPtr result(robot);
return result;
}
-void RobotNode::setJointValue(float q, bool updateTransformations /*= true*/,
- bool clampToLimits /*= true*/)
+void RobotNode::setJointValue(float q, bool updateTransformations,
+ bool clampToLimits)
{
- WriteLock lock(mutex,use_mutex);
VR_ASSERT_MESSAGE( initialized, "Not initialized");
VR_ASSERT_MESSAGE( (!boost::math::isnan(q) && !boost::math::isinf(q)) ,"Not a valid number...");
@@ -169,26 +146,15 @@ void RobotNode::updateTransformationMatrices(const Eigen::Matrix4f &globalPose)
}
-void RobotNode::setPostJointTransformation(const Eigen::Matrix4f &trafo) {
- WriteLock lock(this->mutex,use_mutex);
- postJointTransformation = trafo;
-}
-
-void RobotNode::setPreJointTransformation(const Eigen::Matrix4f &trafo) {
- WriteLock lock(this->mutex,use_mutex);
- preJointTransformation = trafo;
-}
-
void RobotNode::applyJointValue()
{
THROW_VR_EXCEPTION_IF(!initialized, "Not initialized");
updateTransformationMatrices();
- std::vector< RobotNodePtr > children = this->getChildren(); //Stefan
+ std::vector< RobotNodePtr > children = this->getChildren();
for (std::vector< RobotNodePtr >::iterator i = children.begin(); i!= children.end(); i++ )
(*i)->applyJointValue();
- //std::for_each(children.begin(), children.end(), boost::mem_fn(&RobotNode::applyJointValue));
}
void RobotNode::applyJointValue(const Eigen::Matrix4f &globalPose)
@@ -197,10 +163,9 @@ void RobotNode::applyJointValue(const Eigen::Matrix4f &globalPose)
updateTransformationMatrices(globalPose);
- std::vector< RobotNodePtr > children = this->getChildren(); //Stefan
+ std::vector< RobotNodePtr > children = this->getChildren();
for (std::vector< RobotNodePtr >::iterator i = children.begin(); i!= children.end(); i++ )
(*i)->applyJointValue();
- //std::for_each(children.begin(), children.end(), boost::mem_fn(&RobotNode::applyJointValue));
}
void RobotNode::collectAllRobotNodes( std::vector< RobotNodePtr > &storeNodes )
@@ -215,13 +180,12 @@ void RobotNode::collectAllRobotNodes( std::vector< RobotNodePtr > &storeNodes )
float RobotNode::getJointValue() const
{
- ReadLock lock(this->mutex,use_mutex);
+ ReadLockPtr lock = getRobot()->getReadLock();
return jointValue;
}
void RobotNode::respectJointLimits( float &jointValue ) const
{
- WriteLock lock(this->mutex,use_mutex);
if (jointValue < jointLimitLo)
jointValue = jointLimitLo;
if (jointValue > jointLimitHi)
@@ -230,7 +194,7 @@ void RobotNode::respectJointLimits( float &jointValue ) const
bool RobotNode::checkJointLimits( float jointValue, bool verbose ) const
{
- ReadLock lock(this->mutex,use_mutex);
+ ReadLockPtr lock = getRobot()->getReadLock();
bool res = true;
if (jointValue < jointLimitLo)
res = false;
@@ -245,17 +209,11 @@ void RobotNode::setGlobalPose( const Eigen::Matrix4f &pose )
THROW_VR_EXCEPTION("Use setJointValues to control the position of a RobotNode");
}
-void RobotNode::setThreadsafe(bool mode){
- this->use_mutex = mode;
-}
-
-
void RobotNode::print( bool printChildren, bool printDecoration ) const
{
- ReadLock lock(this->mutex,use_mutex);
+ ReadLockPtr lock = getRobot()->getReadLock();
if (printDecoration)
cout << "******** RobotNode ********" << endl;
- cout << "* Thread-safe: " << use_mutex << endl;
cout << "* Name: " << name << endl;
cout << "* Parent: " << this->getParentName() << endl;
cout << "* Children: ";
@@ -281,7 +239,7 @@ void RobotNode::print( bool printChildren, bool printDecoration ) const
cout << " a:" << optionalDHParameter.aMM() << ", d:" << optionalDHParameter.dMM() << ", alpha:" << optionalDHParameter.alphaRadian() << ", theta:" << optionalDHParameter.thetaRadian() << endl;
} else
cout << "* DH parameters: not specified." << endl;
- cout << "* visualisation model: " <<endl;
+ cout << "* visualization model: " <<endl;
if (visualizationModel)
visualizationModel->print();
else
@@ -330,7 +288,7 @@ void RobotNode::addChildNode( RobotNodePtr child )
bool RobotNode::hasChildNode( const RobotNodePtr child, bool recursive ) const
{
- std::vector< RobotNodePtr > children = this->getChildren(); //Stefan
+ std::vector< RobotNodePtr > children = this->getChildren();
for (unsigned int i = 0; i < children.size(); i++)
{
if (children[i] == child)
@@ -349,7 +307,7 @@ bool RobotNode::hasChildNode( const std::string &child, bool recursive ) const
RobotPtr rob(robot);
for (unsigned int i=0; i<this->getChildrenNames().size(); i++)
{
- THROW_VR_EXCEPTION_IF(!rob, "no robot" );
+ VR_ASSERT(rob);
if (this->getChildrenNames()[i] == child)
return true;
@@ -365,7 +323,7 @@ bool RobotNode::hasChildNode( const std::string &child, bool recursive ) const
RobotNodePtr RobotNode::clone( RobotPtr newRobot, bool cloneChildren, RobotNodePtr initializeWithParent, CollisionCheckerPtr colChecker )
{
- ReadLock lock(this->mutex,use_mutex);
+ ReadLockPtr lock = getRobot()->getReadLock();
if (!newRobot)
{
VR_ERROR << "Attempting to clone RobotNode for invalid robot";
@@ -422,13 +380,13 @@ RobotNodePtr RobotNode::clone( RobotPtr newRobot, bool cloneChildren, RobotNodeP
float RobotNode::getJointLimitLo()
{
- ReadLock lock(this->mutex,use_mutex);
+ ReadLockPtr lock = getRobot()->getReadLock();
return jointLimitLo;
}
float RobotNode::getJointLimitHi()
{
- ReadLock lock(this->mutex,use_mutex);
+ ReadLockPtr lock = getRobot()->getReadLock();
return jointLimitHi;
}
@@ -485,7 +443,7 @@ void RobotNode::showCoordinateSystem( bool enable, float scaling, std::string *t
void RobotNode::showStructure( bool enable, const std::string &visualizationType)
{
- ReadLock lock(this->mutex,use_mutex);
+ ReadLockPtr lock = getRobot()->getReadLock();
if (!enable && !visualizationModel)
return; // nothing to do
@@ -555,7 +513,6 @@ VirtualRobot::RobotNodePtr RobotNode::getParent()
void RobotNode::setJointLimits( float lo, float hi )
{
- WriteLock lock(mutex,use_mutex);
jointLimitLo = lo;
jointLimitHi = hi;
}
@@ -590,5 +547,23 @@ float RobotNode::getMaxTorque()
return maxTorque;
}
+void RobotNode::updateVisualizationPose( const Eigen::Matrix4f &globalPose, bool updateChildren )
+{
+ {
+ this->globalPose = globalPose;// * getPreJointTransformation();
+ globalPosePostJoint = this->globalPose*getPostJointTransformation();
+ }
+ // update collision and visualization model
+ // here we do not consider the postJointTransformation, since it already defines the transformation to the next joint.
+ SceneObject::setGlobalPose(this->globalPose);
+
+ if (updateChildren)
+ {
+ std::vector< RobotNodePtr > children = this->getChildren();
+ for (std::vector< RobotNodePtr >::iterator i = children.begin(); i!= children.end(); i++ )
+ (*i)->applyJointValue();
+ }
+}
+
} // namespace VirtualRobot
diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h
index 2b372c81b31ac28eb27074c258358043307a87ee..5daaf118260ad250e93fbda2109a2f717eeffd2d 100644
--- a/VirtualRobot/Nodes/RobotNode.h
+++ b/VirtualRobot/Nodes/RobotNode.h
@@ -45,7 +45,7 @@
namespace VirtualRobot
{
class Robot;
-
+class RobotNodeActuator;
/*!
Each RobotNode owns three transformations:
* preJointTransformation: This transformation is fixed.
@@ -60,7 +60,9 @@ class Robot;
class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNode : public boost::enable_shared_from_this<RobotNode>, public SceneObject
{
public:
+ friend class VirtualRobot::Robot;
friend class RobotFactory;
+ friend class RobotNodeActuator;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/*!
@@ -83,25 +85,9 @@ public:
virtual ~RobotNode();
- RobotPtr getRobot();
-
- /*!
- Set the joint value.
- \param q The joint value.
- \param updateTransformations When true, the transformation matrices of this joint and all child joints are updated (by calling applyJointValue()).
- \param clampToLimits Consider joint limits. When false an exception is thrown in case of invalid values.
- */
- virtual void setJointValue(float q, bool updateTransformations = true, bool clampToLimits = true);
-
- /*!
- Compute/Update the transformations of this joint and all child joints.
- */
- void applyJointValue();
- /*!
- This method is only useful for root nodes (e.g. no parents are available). Then the pose of the robot can be set here.
- */
- virtual void applyJointValue(const Eigen::Matrix4f &globalPos);
+ RobotPtr getRobot() const;
+
/*!
All children and their children (and so on) are collected.
@@ -137,7 +123,6 @@ public:
Usually RobotFactory manages the initialization.
*/
virtual bool initialize(RobotNodePtr parent, bool initializeChildren = false);
- virtual void reset();
/*!
Calling this method will cause an exception, since RobotNodes are controlled via joint values.
@@ -200,7 +185,7 @@ public:
float getJointLimitHi();
/*!
- Set joint limits [rad]
+ Set joint limits [rad].
*/
virtual void setJointLimits(float lo, float hi);
@@ -270,29 +255,54 @@ public:
*/
float getMaxTorque();
- void setThreadsafe(bool);
-
-
//! Forbid cloning method from SceneObject. We need to know the new robot for cloning
SceneObjectPtr clone( const std::string &name, CollisionCheckerPtr colChecker = CollisionCheckerPtr() ) const {THROW_VR_EXCEPTION("Cloning not allowed this way...");}
private: // Use the private setters and getters instead
- float jointValue; //< The joint value
std::vector<std::string> childrenNames;
std::vector< RobotNodePtr > children;
RobotNodeWeakPtr parent;
- Eigen::Matrix4f preJointTransformation;
- Eigen::Matrix4f postJointTransformation;
+
protected:
+
+ /*!
+ Set the joint value. Only Robots are allowed to do this in order to synchronize the access.
+ If you want to update the joint, call \ref Robot::SetJointValue().
+ \param q The joint value.
+ \param updateTransformations When true, the transformation matrices of this joint and all child joints are updated (by calling applyJointValue()).
+ \param clampToLimits Consider joint limits. When false an exception is thrown in case of invalid values.
+ */
+ virtual void setJointValue(float q, bool updateTransformations = true, bool clampToLimits = true);
+
+ /*!
+ Compute/Update the transformations of this joint and all child joints. This method is called by the robot in order to update the pose matrices.
+ */
+ void applyJointValue();
+ /*!
+ This method is only useful for root nodes (e.g. no parents are available). Then the pose of the robot can be set here.
+ */
+ virtual void applyJointValue(const Eigen::Matrix4f &globalPos);
+
+
+ /*!
+ Can be called by a RobotNodeActuator in order to set the pose of the visualization, which means that the preJointTransform is ignored and
+ the pos eof the visualization can be set directly.
+ This is useful, if the node is actuated externally, i.e. via a physics engine.
+ todo: Protect such updates by a mutex.
+ \param globalPose The new global pose. The joint value is determined from this pose (implemented in derived RobtoNodes).
+ \param updateChildren Usually it is assumed that all RobotNodes are updated this way (updateChildren=false). If not, the children poses can be updated according to this node (updateCHildren=true).
+ */
+ virtual void updateVisualizationPose(const Eigen::Matrix4f &globalPose, bool updateChildren = false);
+
///////////////////////// SETUP ////////////////////////////////////
mutable boost::recursive_mutex mutex;
bool use_mutex;
RobotNode(){};
- virtual void setPostJointTransformation(const Eigen::Matrix4f &trafo);
- virtual void setPreJointTransformation(const Eigen::Matrix4f &trafo);
+ //virtual void setPostJointTransformation(const Eigen::Matrix4f &trafo);
+ //virtual void setPreJointTransformation(const Eigen::Matrix4f &trafo);
virtual std::vector<std::string> getChildrenNames() const {return childrenNames;};
virtual std::string getParentName() const {RobotNodePtr p = parent.lock();if (p) return p->getName(); else return std::string();};
@@ -303,6 +313,8 @@ protected:
float maxVelocity; //! given in m/s
float maxAcceleration; //! given in m/s^2
float maxTorque; //! given in Nm
+ Eigen::Matrix4f preJointTransformation;
+ Eigen::Matrix4f postJointTransformation;
///////////////////////// SETUP ////////////////////////////////////
virtual void updateTransformationMatrices();
@@ -311,7 +323,8 @@ protected:
Eigen::Matrix4f globalPosePostJoint; //< The postJoint transformation applied to transformationJoint. Defines the starting pose for all child joints.
-
+ float jointValue; //< The joint value
+
/*!
Derived classes must implement their clone method here.
*/
diff --git a/VirtualRobot/Nodes/RobotNodeActuator.cpp b/VirtualRobot/Nodes/RobotNodeActuator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..17b3a71e64816888626d962b432fd6f51a6d1de3
--- /dev/null
+++ b/VirtualRobot/Nodes/RobotNodeActuator.cpp
@@ -0,0 +1,22 @@
+
+#include "RobotNodeActuator.h"
+#include "../VirtualRobotException.h"
+
+namespace VirtualRobot {
+
+RobotNodeActuator::RobotNodeActuator(RobotNodePtr node)
+{
+ robotNode = node;
+}
+
+RobotNodeActuator::~RobotNodeActuator()
+{
+}
+
+void RobotNodeActuator::updateVisualizationPose( const Eigen::Matrix4f& pose, bool updateChildren )
+{
+ robotNode->updateVisualizationPose(pose, updateChildren);
+}
+
+
+} // namespace
diff --git a/VirtualRobot/Nodes/RobotNodeActuator.h b/VirtualRobot/Nodes/RobotNodeActuator.h
new file mode 100644
index 0000000000000000000000000000000000000000..f72839c25de7aecbe820cabf8afa704a26e75d3c
--- /dev/null
+++ b/VirtualRobot/Nodes/RobotNodeActuator.h
@@ -0,0 +1,60 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Nikolaus Vahrenkamp
+* @copyright 2012 Nikolaus Vahrenkamp
+* GNU Lesser General Public License
+*
+*/
+#ifndef _VirtualRobot_RobotNodeActuator_h_
+#define _VirtualRobot_RobotNodeActuator_h_
+
+#include "../VirtualRobotImportExport.h"
+
+#include "RobotNode.h"
+#include <Eigen/Core>
+
+namespace VirtualRobot
+{
+/*!
+ An interface definition for RobotNode actuators.
+*/
+class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNodeActuator
+{
+public:
+ /*!
+ Constructor
+ */
+ RobotNodeActuator(RobotNodePtr node); //!< The node to actuate
+
+ /*!
+ */
+ virtual ~RobotNodeActuator();
+
+ virtual void updateVisualizationPose (const Eigen::Matrix4f& pose, bool updateChildren = false);
+
+protected:
+
+ RobotNodePtr robotNode;
+};
+
+
+typedef boost::shared_ptr<RobotNodeActuator> RobotNodeActuatorPtr;
+
+} // namespace VirtualRobot
+
+#endif // _VirtualRobot_RobotNodeActuator_h_
diff --git a/VirtualRobot/Nodes/RobotNodeFixed.cpp b/VirtualRobot/Nodes/RobotNodeFixed.cpp
index 187ab387bd017724adda889ff9492b102b0afda8..17e38ec3aafc2bcd67cc54c03c60d4a9d256a804 100644
--- a/VirtualRobot/Nodes/RobotNodeFixed.cpp
+++ b/VirtualRobot/Nodes/RobotNodeFixed.cpp
@@ -8,8 +8,6 @@
#include <boost/bind.hpp>
#include "../VirtualRobotException.h"
-#include "ConditionedLock.h"
-
namespace VirtualRobot {
RobotNodeFixed::RobotNodeFixed(RobotWeakPtr rob,
@@ -24,8 +22,8 @@ RobotNodeFixed::RobotNodeFixed(RobotWeakPtr rob,
) : RobotNode(rob,name,childrenNames,0.0f,0.0f,visualization,collisionModel,0.0f,p,colChecker)
{
optionalDHParameter.isSet = false;
- this->setPreJointTransformation(preJointTransform);
- this->setPostJointTransformation(postJointTransform);
+ this->preJointTransformation = preJointTransform;
+ this->postJointTransformation = postJointTransform;
}
RobotNodeFixed::RobotNodeFixed(RobotWeakPtr rob,
@@ -61,19 +59,14 @@ RobotNodeFixed::RobotNodeFixed(RobotWeakPtr rob,
RotAlpha(2,1) = sin(alpha);
RotAlpha(2,2) = cos(alpha);
- this->setPreJointTransformation(RotTheta);
- this->setPostJointTransformation(TransD*TransA*RotAlpha);
+ this->preJointTransformation = RotTheta;
+ this->postJointTransformation = TransD*TransA*RotAlpha;
}
RobotNodeFixed::~RobotNodeFixed()
{
}
-void RobotNodeFixed::reset()
-{
- RobotNode::reset();
-}
-
bool RobotNodeFixed::initialize(RobotNodePtr parent, bool initializeChildren)
{
return RobotNode::initialize(parent,initializeChildren);
@@ -81,17 +74,13 @@ bool RobotNodeFixed::initialize(RobotNodePtr parent, bool initializeChildren)
void RobotNodeFixed::updateTransformationMatrices()
{
- {
- WriteLock w(mutex,use_mutex);
-
- if (this->getParent())
- globalPose = this->getParent()->getGlobalPose() * getPreJointTransformation();
- else
- globalPose = getPreJointTransformation();
+ if (this->getParent())
+ globalPose = this->getParent()->getGlobalPose() * getPreJointTransformation();
+ else
+ globalPose = getPreJointTransformation();
- globalPosePostJoint = globalPose*getPostJointTransformation();
+ globalPosePostJoint = globalPose*getPostJointTransformation();
- }
// update collision and visualization model
SceneObject::setGlobalPose(globalPose);
}
@@ -99,17 +88,13 @@ void RobotNodeFixed::updateTransformationMatrices()
void RobotNodeFixed::updateTransformationMatrices(const Eigen::Matrix4f &globalPose)
{
- {
- WriteLock w(mutex,use_mutex);
-
- THROW_VR_EXCEPTION_IF(this->getParent(),"This method could only be called on RobotNodes without parents.");
+ VR_ASSERT_MESSAGE(!(this->getParent()),"This method could only be called on RobotNodes without parents.");
- this->globalPose = globalPose * getPreJointTransformation();
+ this->globalPose = globalPose * getPreJointTransformation();
- globalPosePostJoint = this->globalPose*getPostJointTransformation();
+ globalPosePostJoint = this->globalPose*getPostJointTransformation();
- // update collision and visualization model
- }
+ // update collision and visualization model
SceneObject::setGlobalPose(this->globalPose);
}
@@ -132,7 +117,7 @@ void RobotNodeFixed::print( bool printChildren, bool printDecoration ) const
RobotNodePtr RobotNodeFixed::_clone(const RobotPtr newRobot, const std::vector<std::string> newChildren, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker)
{
- ReadLock lock(mutex,use_mutex);
+ ReadLockPtr lock = getRobot()->getReadLock();
RobotNodePtr result;
diff --git a/VirtualRobot/Nodes/RobotNodeFixed.h b/VirtualRobot/Nodes/RobotNodeFixed.h
index f298bf0c9657fadee0291b90f493d6c81d129348..e1b506f1ae5dc5f1bce66311dcae4503228da559 100644
--- a/VirtualRobot/Nodes/RobotNodeFixed.h
+++ b/VirtualRobot/Nodes/RobotNodeFixed.h
@@ -83,7 +83,6 @@ public:
virtual ~RobotNodeFixed();
virtual bool initialize(RobotNodePtr parent, bool initializeChildren = false);
- virtual void reset();
/*!
Print status information.
diff --git a/VirtualRobot/Nodes/RobotNodePrismatic.cpp b/VirtualRobot/Nodes/RobotNodePrismatic.cpp
index a7aad7a721db0c67e874ddc52cc820bad439a10d..6256d44128e3db35a80da16645175edccfb47150 100644
--- a/VirtualRobot/Nodes/RobotNodePrismatic.cpp
+++ b/VirtualRobot/Nodes/RobotNodePrismatic.cpp
@@ -6,7 +6,6 @@
#include <boost/bind.hpp>
#include <Eigen/Geometry>
#include "../VirtualRobotException.h"
-#include "ConditionedLock.h"
namespace VirtualRobot {
@@ -28,9 +27,11 @@ RobotNodePrismatic::RobotNodePrismatic(RobotWeakPtr rob,
{
initialized = false;
optionalDHParameter.isSet = false;
- this->setPreJointTransformation(preJointTransform);
+ this->preJointTransformation = preJointTransform;
+ this->postJointTransformation = postJointTransform;
+ //this->setPreJointTransformation(preJointTransform);
this->jointTranslationDirection = translationDirection;
- this->setPostJointTransformation(postJointTransform);
+ //this->setPostJointTransformation(postJointTransform);
}
RobotNodePrismatic::RobotNodePrismatic(RobotWeakPtr rob,
@@ -71,25 +72,17 @@ RobotNodePrismatic::RobotNodePrismatic(RobotWeakPtr rob,
RotAlpha(2,2) = cos(alpha);
// fixed rotation around theta
- this->setPreJointTransformation(RotTheta);
+ this->preJointTransformation = RotTheta;
// joint setup
jointTranslationDirection = Eigen::Vector3f(0,0,1); // translation along the z axis
// compute postJointTransformation
- this->setPostJointTransformation(TransD*TransA*RotAlpha);
+ this->postJointTransformation = TransD*TransA*RotAlpha;
}
RobotNodePrismatic::~RobotNodePrismatic()
{
}
-void RobotNodePrismatic::reset()
-{
- {
- WriteLock w(mutex,use_mutex);
- jointTranslationDirection = Eigen::Vector3f(0,0,1);
- }
- RobotNode::reset();
-}
bool RobotNodePrismatic::initialize(RobotNodePtr parent, bool initializeChildren)
{
@@ -98,19 +91,15 @@ bool RobotNodePrismatic::initialize(RobotNodePtr parent, bool initializeChildren
void RobotNodePrismatic::updateTransformationMatrices()
{
- {
- WriteLock w(mutex,use_mutex);
-
- if (this->getParent())
- globalPose = this->getParent()->getGlobalPose() * this->getPreJointTransformation();
- else
- globalPose = this->getPreJointTransformation();
+ if (this->getParent())
+ globalPose = this->getParent()->getGlobalPose() * this->getPreJointTransformation();
+ else
+ globalPose = this->getPreJointTransformation();
- Eigen::Affine3f tmpT(Eigen::Translation3f((this->getJointValue()+jointValueOffset)*jointTranslationDirection));
- globalPose *= tmpT.matrix();
+ Eigen::Affine3f tmpT(Eigen::Translation3f((this->getJointValue()+jointValueOffset)*jointTranslationDirection));
+ globalPose *= tmpT.matrix();
- globalPosePostJoint = globalPose*this->getPostJointTransformation();
- }
+ globalPosePostJoint = globalPose*this->getPostJointTransformation();
// update collision and visualization model
// here we do not consider the postJointTransformation, since it already defines the transformation to the next joint.
SceneObject::setGlobalPose(globalPose);
@@ -118,18 +107,14 @@ void RobotNodePrismatic::updateTransformationMatrices()
void RobotNodePrismatic::updateTransformationMatrices(const Eigen::Matrix4f &globalPose)
{
- {
- WriteLock w(mutex,use_mutex);
+ VR_ASSERT_MESSAGE(!(this->getParent()),"This method could only be called on RobotNodes without parents.");
- THROW_VR_EXCEPTION_IF(this->getParent(),"This method could only be called on RobotNodes without parents.");
+ this->globalPose = globalPose * getPreJointTransformation();
- this->globalPose = globalPose * getPreJointTransformation();
+ Eigen::Affine3f tmpT(Eigen::Translation3f((this->getJointValue()+jointValueOffset)*jointTranslationDirection));
+ this->globalPose *= tmpT.matrix();
- Eigen::Affine3f tmpT(Eigen::Translation3f((this->getJointValue()+jointValueOffset)*jointTranslationDirection));
- this->globalPose *= tmpT.matrix();
-
- globalPosePostJoint = this->globalPose*getPostJointTransformation();
- }
+ globalPosePostJoint = this->globalPose*getPostJointTransformation();
// update collision and visualization model
// here we do not consider the postJointTransformation, since it already defines the transformation to the next joint.
@@ -138,7 +123,7 @@ void RobotNodePrismatic::updateTransformationMatrices(const Eigen::Matrix4f &glo
void RobotNodePrismatic::print( bool printChildren, bool printDecoration ) const
{
- ReadLock lock(mutex,use_mutex);
+ ReadLockPtr lock = getRobot()->getReadLock();
if (printDecoration)
cout << "******** RobotNodePrismatic ********" << endl;
@@ -159,7 +144,7 @@ void RobotNodePrismatic::print( bool printChildren, bool printDecoration ) const
RobotNodePtr RobotNodePrismatic::_clone(const RobotPtr newRobot, const std::vector<std::string> newChildren, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker)
{
RobotNodePtr result;
- ReadLock lock(mutex,use_mutex);
+ ReadLockPtr lock = getRobot()->getReadLock();
if (optionalDHParameter.isSet)
result.reset(new RobotNodePrismatic(newRobot,name,newChildren, jointLimitLo,jointLimitHi,optionalDHParameter.aMM(),optionalDHParameter.dMM(), optionalDHParameter.alphaRadian(), optionalDHParameter.thetaRadian(),visualizationModel,collisionModel, jointValueOffset,physics,colChecker));
@@ -176,7 +161,7 @@ bool RobotNodePrismatic::isTranslationalJoint() const
Eigen::Vector3f RobotNodePrismatic::getJointTranslationDirection(const SceneObjectPtr coordSystem) const
{
- ReadLock lock(mutex,use_mutex);
+ ReadLockPtr lock = getRobot()->getReadLock();
Eigen::Vector4f result4f = Eigen::Vector4f::Zero();
result4f.segment(0,3) = jointTranslationDirection;
@@ -190,6 +175,30 @@ Eigen::Vector3f RobotNodePrismatic::getJointTranslationDirection(const SceneObje
return result4f.segment(0,3);
}
+void RobotNodePrismatic::updateVisualizationPose( const Eigen::Matrix4f &globalPose, bool updateChildren /*= false*/ )
+{
+ RobotNode::updateVisualizationPose(globalPose,updateChildren);
+
+ // compute the jointValue from pose
+ Eigen::Matrix4f localPose = toLocalCoordinateSystem(globalPose);
+ Eigen::Vector3f v = localPose.block(0,3,3,1);
+
+ // project on directionVector
+ MathTools::Line l(Eigen::Vector3f::Zero(),jointTranslationDirection);
+ Eigen::Vector3f v_on_line = MathTools::nearestPointOnLine(l,v);
+ float dist = v_on_line.norm();
+
+ // check pos/neg direction
+ if (v_on_line.normalized().dot(jointTranslationDirection.normalized()) < 0)
+ {
+ // pointing in opposite direction
+ dist = -dist;
+ }
+
+ // consider offset
+ jointValue = dist - jointValueOffset;
+}
+
} // namespace
diff --git a/VirtualRobot/Nodes/RobotNodePrismatic.h b/VirtualRobot/Nodes/RobotNodePrismatic.h
index d4b112bfe13f3a74a9c69a1d615719b8026b837b..99fc15ecdc53bdabc02a3b6b7a247498283e26bf 100644
--- a/VirtualRobot/Nodes/RobotNodePrismatic.h
+++ b/VirtualRobot/Nodes/RobotNodePrismatic.h
@@ -53,7 +53,7 @@ public:
float jointLimitLo, //!< lower joint limit
float jointLimitHi, //!< upper joint limit
const Eigen::Matrix4f &preJointTransform, //!< This transformation is applied before the translation of the joint is done
- const Eigen::Vector3f &translationDirection, //!< This is the direction of the translation
+ const Eigen::Vector3f &translationDirection, //!< This is the direction of the translation (local)
const Eigen::Matrix4f &postJointTransform, //!< This is an additional transformation, that is applied after the translation is done
VisualizationNodePtr visualization = VisualizationNodePtr(), //!< A visualization model
CollisionModelPtr collisionModel = CollisionModelPtr(), //!< A collision model
@@ -81,7 +81,6 @@ public:
virtual ~RobotNodePrismatic();
virtual bool initialize(RobotNodePtr parent, bool initializeChildren = false);
- virtual void reset();
/*!
Print status information.
@@ -91,18 +90,25 @@ public:
virtual bool isTranslationalJoint() const;
/*!
- Standard: In local coord system.
+ In global coord system.
\param coordSystem When not set the direction is transformed to global coord system. Otherwise any scene object can be used as coord system.
*/
Eigen::Vector3f getJointTranslationDirection(const SceneObjectPtr coordSystem = SceneObjectPtr()) const;
protected:
+ /*!
+ Can be called by a RobotNodeActuator in order to set the pose of this node.
+ This is useful, if the node is actuated externally, i.e. via a physics engine.
+ \param globalPose The new global pose. The joint value is determined from this pose (implemented in derived RobtoNodes).
+ \param updateChildren Usually it is assumed that all RobotNodes are updated this way (updateChildren=false). If not, the children poses can be updated according to this node (updateCHildren=true).
+ */
+ virtual void updateVisualizationPose(const Eigen::Matrix4f &globalPose, bool updateChildren = false);
RobotNodePrismatic(){};
virtual void updateTransformationMatrices();
virtual void updateTransformationMatrices(const Eigen::Matrix4f &globalPose);
- Eigen::Vector3f jointTranslationDirection; // used when ePrismaticJoint
+ Eigen::Vector3f jointTranslationDirection; // used when ePrismaticJoint (local coord system)
virtual RobotNodePtr _clone(const RobotPtr newRobot, const std::vector<std::string> newChildren, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker);
diff --git a/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp b/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp
index b8b324252a01bac9b6493221db1689e5913836c0..075aef23ac8ec92e12482e78240c35e38d5f7aef 100644
--- a/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp
+++ b/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp
@@ -44,20 +44,6 @@ RobotNodePtr RobotNodePrismaticFactory::createRobotNodeDH(RobotPtr robot, const
RobotNodePtr robotNode(new RobotNodePrismatic(robot, nodeName, childrenNames, limitLow, limitHigh, dhParameters.aMM(), dhParameters.dMM(), dhParameters.alphaRadian(), dhParameters.thetaRadian(), visualizationModel, collisionModel, jointValueOffset,p));
return robotNode;
-
- /*
- // fixed rotation around theta
- Eigen::Matrix4f preJointTransformation = dhParameters.thetaRotationRadian();
- // joint setup
- Eigen::Vector3f jointTranslationDirection = Eigen::Vector3f(0,0,1); // translation along the z axis
- // compute postJointTransformation
- Eigen::Matrix4f postJointTransformation = dhParameters.aTranslation() * dhParameters.alphaRotationRadian();
-
- // unused but necessary for calling the method
- Eigen::Vector3f jointRotationAxis = Eigen::Vector3f::Zero();
-
- return createRobotNode(robot, nodeName, childrenNames, visualizationModel, collisionModel, limitLow, limitHigh, jointValueOffset, preJointTransformation, jointRotationAxis, postJointTransformation, jointTranslationDirection);
- */
}
diff --git a/VirtualRobot/Nodes/RobotNodeRevolute.cpp b/VirtualRobot/Nodes/RobotNodeRevolute.cpp
index e2a353c77faf490fcd9a16cefc885b7a7bed5cad..e7d2898207476c59a44e2efe633892281012b072 100644
--- a/VirtualRobot/Nodes/RobotNodeRevolute.cpp
+++ b/VirtualRobot/Nodes/RobotNodeRevolute.cpp
@@ -7,8 +7,6 @@
#include <Eigen/Geometry>
#include "../VirtualRobotException.h"
-#include "ConditionedLock.h"
-
namespace VirtualRobot {
@@ -30,9 +28,9 @@ RobotNodeRevolute::RobotNodeRevolute(RobotWeakPtr rob,
{
initialized = false;
optionalDHParameter.isSet = false;
- this->setPreJointTransformation(preJointTransform);
- jointRotationAxis = axis;
- this->setPostJointTransformation(postJointTransform);
+ this->preJointTransformation = preJointTransform;
+ this->jointRotationAxis = axis;
+ this->postJointTransformation = postJointTransform;
/*preJointTransformation = Eigen::Matrix4f::Identity();
postJointTransformation = Eigen::Matrix4f::Identity();
postJointTransformation.block<3,1>(0,3) = postJointTranslation;*/
@@ -78,9 +76,9 @@ RobotNodeRevolute::RobotNodeRevolute(RobotWeakPtr rob,
RotAlpha(2,1) = sin(alpha);
RotAlpha(2,2) = cos(alpha);
- this->setPreJointTransformation(RotTheta);//Eigen::Matrix4f::Identity(); // no pre transformation->why?
- jointRotationAxis = Eigen::Vector3f(0,0,1); // rotation around z axis
- this->setPostJointTransformation(TransD*TransA*RotAlpha);
+ this->preJointTransformation = RotTheta;//Eigen::Matrix4f::Identity(); // no pre transformation->why?
+ this->jointRotationAxis = Eigen::Vector3f(0,0,1); // rotation around z axis
+ this->postJointTransformation = TransD*TransA*RotAlpha;
}
@@ -88,15 +86,6 @@ RobotNodeRevolute::~RobotNodeRevolute()
{
}
-void RobotNodeRevolute::reset()
-{
- {
- WriteLock w(mutex,use_mutex);
- jointRotationAxis = Eigen::Vector3f(1,0,0);
- }
- RobotNode::reset();
-}
-
bool RobotNodeRevolute::initialize(RobotNodePtr parent, bool initializeChildren)
{
return RobotNode::initialize(parent,initializeChildren);
@@ -104,20 +93,17 @@ bool RobotNodeRevolute::initialize(RobotNodePtr parent, bool initializeChildren)
void RobotNodeRevolute::updateTransformationMatrices()
{
- {
-
- WriteLock w(mutex,use_mutex);
-
- if (this->getParent())
- globalPose = this->getParent()->getGlobalPose() * getPreJointTransformation();
- else
- globalPose = getPreJointTransformation();
-
- Eigen::Affine3f tmpT(Eigen::AngleAxisf(this->getJointValue()+jointValueOffset,jointRotationAxis));
- globalPose *= tmpT.matrix();
-
- globalPosePostJoint = globalPose*getPostJointTransformation();
- }
+
+ if (this->getParent())
+ globalPose = this->getParent()->getGlobalPose() * getPreJointTransformation();
+ else
+ globalPose = getPreJointTransformation();
+
+ Eigen::Affine3f tmpT(Eigen::AngleAxisf(this->getJointValue()+jointValueOffset,jointRotationAxis));
+ globalPose *= tmpT.matrix();
+
+ globalPosePostJoint = globalPose*getPostJointTransformation();
+
// update collision and visualization model
// here we do not consider the postJointTransformation, since it already defines the transformation to the next joint.
SceneObject::setGlobalPose(globalPose);
@@ -125,17 +111,14 @@ void RobotNodeRevolute::updateTransformationMatrices()
void RobotNodeRevolute::updateTransformationMatrices(const Eigen::Matrix4f &globalPose)
{
- THROW_VR_EXCEPTION_IF(this->getParent(),"This method could only be called on RobotNodes without parents.");
- {
-
- WriteLock w(mutex,use_mutex);
- this->globalPose = globalPose * getPreJointTransformation();
+ VR_ASSERT_MESSAGE(!(this->getParent()),"This method could only be called on RobotNodes without parents.");
+ this->globalPose = globalPose * getPreJointTransformation();
- Eigen::Affine3f tmpT(Eigen::AngleAxisf(this->getJointValue()+jointValueOffset,jointRotationAxis));
- this->globalPose *= tmpT.matrix();
+ Eigen::Affine3f tmpT(Eigen::AngleAxisf(this->getJointValue()+jointValueOffset,jointRotationAxis));
+ this->globalPose *= tmpT.matrix();
+
+ globalPosePostJoint = this->globalPose*getPostJointTransformation();
- globalPosePostJoint = this->globalPose*getPostJointTransformation();
- }
// update collision and visualization model
// here we do not consider the postJointTransformation, since it already defines the transformation to the next joint.
SceneObject::setGlobalPose(this->globalPose);
@@ -144,7 +127,7 @@ void RobotNodeRevolute::updateTransformationMatrices(const Eigen::Matrix4f &glob
void RobotNodeRevolute::print( bool printChildren, bool printDecoration ) const
{
- ReadLock lock(mutex,use_mutex);
+ ReadLockPtr lock = getRobot()->getReadLock();
if (printDecoration)
cout << "******** RobotNodeRevolute ********" << endl;
@@ -163,7 +146,7 @@ void RobotNodeRevolute::print( bool printChildren, bool printDecoration ) const
RobotNodePtr RobotNodeRevolute::_clone(const RobotPtr newRobot, const std::vector<std::string> newChildren, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker)
{
- ReadLock lock(mutex,use_mutex);
+ ReadLockPtr lock = getRobot()->getReadLock();
RobotNodePtr result;
if (optionalDHParameter.isSet)
@@ -180,7 +163,7 @@ bool RobotNodeRevolute::isRotationalJoint() const
Eigen::Vector3f RobotNodeRevolute::getJointRotationAxis(const SceneObjectPtr coordSystem) const
{
- ReadLock lock(mutex,use_mutex);
+ ReadLockPtr lock = getRobot()->getReadLock();
Eigen::Vector4f result4f = Eigen::Vector4f::Zero();
result4f.segment(0,3) = jointRotationAxis;
result4f = getGlobalPoseJoint()*result4f;
@@ -194,4 +177,29 @@ Eigen::Vector3f RobotNodeRevolute::getJointRotationAxis(const SceneObjectPtr coo
return result4f.block(0,0,3,1);
}
+void RobotNodeRevolute::updateVisualizationPose( const Eigen::Matrix4f &globalPose, bool updateChildren /*= false*/ )
+{
+ RobotNode::updateVisualizationPose(globalPose,updateChildren);
+
+ // compute the jointValue from pose
+
+ // jointRotationAxis is given in local joint coord system
+ // -> we need the pose in joint coord system
+ Eigen::Matrix4f localPose = getGlobalPoseJoint().inverse() * globalPose; //toLocalCoordinateSystem(globalPose);
+
+ Eigen::Vector3f localAxis;
+ float angle;
+ MathTools::eigen4f2axisangle(localPose,localAxis,angle);
+
+ // check pos/neg direction
+ if (jointRotationAxis.normalized().dot(localAxis.normalized()) < 0)
+ {
+ // pointing in opposite direction
+ angle = -angle;
+ }
+
+ // consider offset
+ jointValue = angle - jointValueOffset;
+}
+
} // namespace
diff --git a/VirtualRobot/Nodes/RobotNodeRevolute.h b/VirtualRobot/Nodes/RobotNodeRevolute.h
index 56380a18d3d33a6d458a07be39dcb3ce99a7a6a5..ea6f54dc23aa16102a5e09cc8ba8032eedaf30b2 100644
--- a/VirtualRobot/Nodes/RobotNodeRevolute.h
+++ b/VirtualRobot/Nodes/RobotNodeRevolute.h
@@ -54,7 +54,7 @@ public:
float jointLimitLo, //!< lower joint limit
float jointLimitHi, //!< upper joint limit
const Eigen::Matrix4f &preJointTransform, //!< This transformation is applied before the translation of the joint is done
- const Eigen::Vector3f &axis, //!< The rotation axis
+ const Eigen::Vector3f &axis, //!< The rotation axis (in local joint coord system)
const Eigen::Matrix4f &postJointTransform, //!< This is an additional transformation, that is applied after the rotation is done
VisualizationNodePtr visualization = VisualizationNodePtr(), //!< A visualization model
CollisionModelPtr collisionModel = CollisionModelPtr(), //!< A collision model
@@ -82,7 +82,6 @@ public:
virtual ~RobotNodeRevolute();
virtual bool initialize(RobotNodePtr parent, bool initializeChildren = false);
- virtual void reset();
/*!
Print status information.
@@ -96,13 +95,21 @@ public:
*/
Eigen::Vector3f getJointRotationAxis(const SceneObjectPtr coordSystem = SceneObjectPtr()) const;
protected:
+ /*!
+ Can be called by a RobotNodeActuator in order to set the pose of this node.
+ This is useful, if the node is actuated externally, i.e. via a physics engine.
+ \param globalPose The new global pose. The joint value is determined from this pose (implemented in derived RobtoNodes).
+ \param updateChildren Usually it is assumed that all RobotNodes are updated this way (updateChildren=false). If not, the children poses can be updated according to this node (updateCHildren=true).
+ */
+ virtual void updateVisualizationPose(const Eigen::Matrix4f &globalPose, bool updateChildren = false);
+
RobotNodeRevolute(){};
virtual void updateTransformationMatrices();
virtual void updateTransformationMatrices(const Eigen::Matrix4f &globalPose);
- Eigen::Vector3f jointRotationAxis; // eRevoluteJoint
+ Eigen::Vector3f jointRotationAxis; // eRevoluteJoint (given in local joint coord system)
virtual RobotNodePtr _clone(const RobotPtr newRobot, const std::vector<std::string> newChildren, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker);
};
diff --git a/VirtualRobot/Obstacle.h b/VirtualRobot/Obstacle.h
index 034040037309dd9c6fde13785067c3d0715e841b..0ee9b2c11206c297e873ae949a84c1811ac954e3 100644
--- a/VirtualRobot/Obstacle.h
+++ b/VirtualRobot/Obstacle.h
@@ -64,7 +64,7 @@ public:
\param height The height of the box.
\param depth The depth of the box.
\param color Specify the color.
- \param visualizationType Here the typo of visualization can be specified (e.g. "Inventor"). If empty, the first registered visualization type (which is usually the only one) is used.
+ \param visualizationType Here the type of visualization can be specified (e.g. "Inventor"). If empty, the first registered visualization type (which is usually the only one) is used.
\param colChecker Only needed if you plan to use the collision checker in parallel. If not given, the object is registered with the global singleton collision checker.
*/
static ObstaclePtr createBox(float width, float height, float depth, VisualizationFactory::Color color = VisualizationFactory::Color::Red(), std::string visualizationType = "", CollisionCheckerPtr colChecker = CollisionCheckerPtr());
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index ac7568e66ce9d537083699ca3d13d6ab7e86003d..3da8815981a480a4c39f30afd318e3f3c18ce891 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -1,5 +1,6 @@
#include "Robot.h"
+#include "Trajectory.h"
#include "VirtualRobotException.h"
#include "CollisionDetection/CollisionChecker.h"
#include "EndEffector/EndEffector.h"
@@ -8,11 +9,11 @@
namespace VirtualRobot {
Robot::Robot(const std::string &name, const std::string &type)
-
{
this->name = name;
this->type = type;
updateVisualization = true;
+ use_mutex = true;
}
Robot::Robot()
@@ -47,7 +48,7 @@ void LocalRobot::setRootNode( RobotNodePtr node )
} else
{
// create all globalposes
- rootNode->applyJointValue(globalPose);
+ applyJointValues();
std::vector< RobotNodePtr > allNodes;
node->collectAllRobotNodes( allNodes );
@@ -63,13 +64,9 @@ void LocalRobot::setRootNode( RobotNodePtr node )
}
}
-void Robot::setThreadsafe(bool flag){
-
- std::vector< RobotNodePtr > nodes = this->getRobotNodes();
- BOOST_FOREACH(RobotNodePtr node, nodes){
- node->setThreadsafe(flag);
- }
-
+void Robot::setThreadsafe(bool flag)
+{
+ use_mutex = flag;
}
RobotNodePtr LocalRobot::getRobotNode( const std::string &robotNodeName )
@@ -331,9 +328,17 @@ RobotNodePtr LocalRobot::getRootNode()
*/
void Robot::applyJointValues()
{
+ WriteLock(mutex,use_mutex);
this->getRootNode()->applyJointValue(this->getGlobalPose());
}
+/**
+ * Can be called internally, when lock is already set.
+ */
+void Robot::applyJointValuesNoLock()
+{
+ this->getRootNode()->applyJointValue(this->getGlobalPose());
+}
/**
* This method stores all nodes belonging to the robot in \p storeNodes.
@@ -487,12 +492,14 @@ VirtualRobot::CollisionCheckerPtr Robot::getCollisionChecker()
void LocalRobot::setGlobalPose(const Eigen::Matrix4f &globalPose )
{
+ WriteLock(mutex,use_mutex);
this->globalPose = globalPose;
- applyJointValues();
+ applyJointValuesNoLock();
}
Eigen::Matrix4f LocalRobot::getGlobalPose()
{
+ ReadLock(mutex,use_mutex);
return globalPose;
}
@@ -659,9 +666,11 @@ VirtualRobot::RobotConfigPtr Robot::getConfig()
bool Robot::setConfig( RobotConfigPtr c )
{
- if (c)
- return c->applyToRobot(shared_from_this());
- return false;
+ if (!c)
+ return false;
+
+ setJointValues(c);
+ return true;
}
void Robot::setFilename( const std::string &filename )
@@ -674,6 +683,116 @@ std::string Robot::getFilename()
return filename;
}
+ReadLockPtr Robot::getReadLock()
+{
+ return ReadLockPtr(new ReadLock(mutex,use_mutex));
+}
+
+void Robot::setJointValue( const std::string &nodeName, float jointValue )
+{
+ RobotNodePtr rn = getRobotNode(nodeName);
+ setJointValue(rn, jointValue);
+}
+
+void Robot::setJointValue( RobotNodePtr rn, float jointValue )
+{
+ VR_ASSERT(rn);
+ WriteLock(mutex,use_mutex);
+ rn->setJointValue(jointValue);
+}
+
+void Robot::setJointValues( const std::map< std::string, float > &jointValues )
+{
+ WriteLock(mutex,use_mutex);
+
+ std::map< std::string, float >::const_iterator it = jointValues.begin();
+ while (it != jointValues.end())
+ {
+ RobotNodePtr rn = getRobotNode(it->first);
+ VR_ASSERT(rn);
+ rn->setJointValue(it->second,false);
+ it++;
+ }
+ applyJointValuesNoLock();
+}
+
+void Robot::setJointValues( RobotNodeSetPtr rns, const std::vector<float> &jointValues )
+{
+ VR_ASSERT(rns);
+ THROW_VR_EXCEPTION_IF(jointValues.size() != rns->getSize(), "Wrong vector dimension (robotNodes:" << rns->getSize() << ", jointValues: " << jointValues.size() << ")" << endl);
+ WriteLock(mutex,use_mutex);
+ for (unsigned int i=0;i<rns->getSize();i++)
+ {
+ rns->getNode(i)->setJointValue(jointValues[i],false);
+ }
+ rns->getKinematicRoot()->applyJointValue();
+}
+
+void Robot::setJointValues( RobotNodeSetPtr rns, const Eigen::VectorXf &jointValues )
+{
+ VR_ASSERT(rns);
+ THROW_VR_EXCEPTION_IF(jointValues.size() != rns->getSize(), "Wrong vector dimension (robotNodes:" << rns->getSize() << ", jointValues: " << jointValues.size() << ")" << endl);
+ WriteLock(mutex,use_mutex);
+ for (unsigned int i=0;i<rns->getSize();i++)
+ {
+ rns->getNode(i)->setJointValue(jointValues[i],false);
+ }
+ rns->getKinematicRoot()->applyJointValue();
+}
+
+void Robot::setJointValues( RobotConfigPtr config )
+{
+ VR_ASSERT(config);
+ WriteLock(mutex,use_mutex);
+ std::map < std::string, float > jv = config->getRobotNodeJointValueMap();
+ std::map< std::string, float >::const_iterator i = jv.begin();
+
+ while (i != jv.end())
+ {
+ RobotNodePtr rn = getRobotNode(i->first);
+ VR_ASSERT(rn);
+ rn->setJointValue(i->second,false);
+ i++;
+ }
+ applyJointValuesNoLock();
+}
+
+void Robot::setJointValues( TrajectoryPtr trajectory, float t )
+{
+ VR_ASSERT(trajectory);
+ Eigen::VectorXf c;
+ trajectory->interpolate(t,c);
+ setJointValues(trajectory->getRobotNodeSet(),c);
+}
+
+void Robot::setJointValues( RobotNodeSetPtr rns, RobotConfigPtr config )
+{
+ VR_ASSERT(rns);
+ VR_ASSERT(config);
+ WriteLock(mutex,use_mutex);
+ const std::vector<RobotNodePtr> robotNodes = rns->getAllRobotNodes();
+ for (unsigned int i=0;i<robotNodes.size();i++)
+ {
+ if (config->hasConfig(robotNodes[i]->getName()))
+ robotNodes[i]->setJointValue(config->getConfig(robotNodes[i]->getName()), false, true);
+ }
+ if (rns->getKinematicRoot() && rns->getKinematicRoot()!=getRootNode())
+ rns->getKinematicRoot()->applyJointValue();
+ else
+ {
+ applyJointValuesNoLock();
+ }
+}
+
+void Robot::setJointValues( const std::vector<RobotNodePtr> rn, const std::vector<float> &jointValues )
+{
+ VR_ASSERT(rn.size()==jointValues.size());
+ WriteLock(mutex,use_mutex);
+ for (size_t i=0;i<rn.size();i++)
+ rn[i]->setJointValue(jointValues[i],false);
+ applyJointValuesNoLock();
+}
+
} // namespace VirtualRobot
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index e81e5235c0666481d8af0609d6f3d1e0eb47f196..90f1cf6881ae265dcfcc642de89352e6908fff6d 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -27,8 +27,10 @@
#include "Nodes/RobotNode.h"
#include "RobotNodeSet.h"
#include "RobotConfig.h"
+#include "Nodes/ConditionedLock.h"
#include <boost/enable_shared_from_this.hpp>
+#include <boost/thread/shared_mutex.hpp>
#include <boost/type_traits/is_base_of.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/bind.hpp>
@@ -237,6 +239,78 @@ public:
*/
std::string getFilename();
+ /*!
+ This readlock can be used to protect data access. It returns a <multiple reads> / <single write> lock.
+ */
+ ReadLockPtr getReadLock();
+
+ /*!
+ Set a joint value [rad].
+ The internal matrices and visualizations are updated accordingly.
+ If you intend to update multiple joints, use \ref setJointValues for faster access.
+ */
+ void setJointValue(RobotNodePtr rn, float jointValue);
+ /*!
+ Set a joint value [rad].
+ The internal matrices and visualizations are updated accordingly.
+ If you intend to update multiple joints, use \ref setJointValues for faster access.
+ */
+ void setJointValue(const std::string &nodeName, float jointValue);
+
+ /*!
+ Set a joint values [rad].
+ The complete robot is updated to apply the new joint values.
+ \param jointValues A map containing RobotNode names with according values.
+ */
+ void setJointValues(const std::map< std::string, float > &jointValues );
+ /*!
+ Set a joint values [rad].
+ The complete robot is updated to apply the new joint values.
+ \param jointValues A map containing RobotNodes with according values.
+ */
+ void setJointValues(const std::map< RobotNodePtr, float > &jointValues );
+ /*!
+ Set a joint values [rad].
+ The subpart of the robot, defined by the start joint (kinematicRoot) of rns, is updated to apply the new joint values.
+ \param rns The RobotNodeSet defines the joints
+ \param jointValues A vector with joint values, size must be equal to rns.
+ */
+ void setJointValues(RobotNodeSetPtr rns, const std::vector<float> &jointValues);
+ /*!
+ Set a joint values [rad].
+ The complete robot is updated to apply the new joint values.
+ \param rn The RobotNodes
+ \param jointValues A vector with joint values, size must be equal to rn.
+ */
+ void setJointValues(const std::vector<RobotNodePtr> rn, const std::vector<float> &jointValues);
+ /*!
+ Set a joint values [rad].
+ The subpart of the robot, defined by the start joint (kinematicRoot) of rns, is updated to apply the new joint values.
+ \param rns The RobotNodeSet defines the joints
+ \param jointValues A vector with joint values, size must be equal to rns.
+ */
+ void setJointValues(RobotNodeSetPtr rns, const Eigen::VectorXf &jointValues);
+ /*!
+ Set a joint values [rad].
+ The complete robot is updated to apply the new joint values.
+ \param config The RobotConfig defines the RobotNodes and joint values.
+ */
+ void setJointValues(RobotConfigPtr config);
+ /*!
+ Set a joint values [rad].
+ Only those joints in config are affected which are present in rns.
+ The subpart of the robot, defined by the start joint (kinematicRoot) of rns, is updated to apply the new joint values.
+ \param rns Only joints in this rns are updated.
+ \param config The RobotConfig defines the RobotNodes and joint values.
+ */
+ void setJointValues(RobotNodeSetPtr rns, RobotConfigPtr config);
+ /*!
+ Apply configuration of trajectory at time t
+ \param trajectory The trajectory
+ \param t The time (0<=t<=1)
+ */
+ void setJointValues(TrajectoryPtr trajectory, float t);
+
protected:
Robot();
/*!
@@ -246,12 +320,19 @@ protected:
*/
void createVisualizationFromCollisionModels();
+ //! It is assumed that the mutex is already set
+ void applyJointValuesNoLock();
+
+
std::string filename; // RobotIO stores the filename here
std::string name;
std::string type;
bool updateVisualization;
+ boost::recursive_mutex mutex;
+ bool use_mutex;
+
};
/**
@@ -312,8 +393,6 @@ protected:
std::map< std::string, RobotNodePtr > robotNodeMap;
std::map< std::string, RobotNodeSetPtr > robotNodeSetMap;
std::map< std::string, EndEffectorPtr > endEffectorMap;
-
-
};
diff --git a/VirtualRobot/RobotConfig.cpp b/VirtualRobot/RobotConfig.cpp
index 362032d383bf738d33f1c957c1d2e6553726342a..614b923aaa06c54c4ef96622bc165e7d735937cb 100644
--- a/VirtualRobot/RobotConfig.cpp
+++ b/VirtualRobot/RobotConfig.cpp
@@ -144,7 +144,7 @@ RobotConfigPtr RobotConfig::clone( RobotPtr newRobot )
return result;
}
-
+/*
bool RobotConfig::setJointValues()
{
RobotPtr r = robot.lock();
@@ -159,7 +159,7 @@ bool RobotConfig::setJointValues()
}
r->applyJointValues();
return true;
-}
+}*/
bool RobotConfig::hasConfig( const std::string & name ) const
{
@@ -216,7 +216,7 @@ std::map < std::string, float > RobotConfig::getRobotNodeJointValueMap()
}
return result;
}
-
+/*
bool RobotConfig::applyToRobot( RobotPtr r )
{
if (!r)
@@ -245,7 +245,7 @@ bool RobotConfig::applyToRobot( RobotPtr r )
}
r->applyJointValues();
return true;
-}
+}*/
std::string RobotConfig::getXMLString(int tabs)
{
diff --git a/VirtualRobot/RobotConfig.h b/VirtualRobot/RobotConfig.h
index 2aa98b85d01fb7f0c88b6af44d9e4111932275c1..436ab75e6e13474bc95daa75f02740ef60431080 100644
--- a/VirtualRobot/RobotConfig.h
+++ b/VirtualRobot/RobotConfig.h
@@ -92,7 +92,7 @@ public:
RobotNodes that are not stored in this RobotConfig are not affected.
\return True on success. False if robot is not present any more (may happen due to the use of weak pointers).
*/
- bool setJointValues();
+ //bool setJointValues();
/*!
Check if a configuration for a RobotNode with name is stored.
@@ -117,7 +117,7 @@ public:
Usually setJointValues() is sufficient for applying the joint values. But in some cases one might want to
apply the joint values to a cloned robot. Therefore this method can be used.
*/
- bool applyToRobot(RobotPtr r);
+ //bool applyToRobot(RobotPtr r);
/*!
Create an XML string that defines this object.
diff --git a/VirtualRobot/RobotNodeSet.cpp b/VirtualRobot/RobotNodeSet.cpp
index 05dc802918c8105e765d19478de16bee748f5edd..f75503286070324a776f8d86fe50d9b10665d536 100644
--- a/VirtualRobot/RobotNodeSet.cpp
+++ b/VirtualRobot/RobotNodeSet.cpp
@@ -22,17 +22,13 @@ RobotNodeSet::RobotNodeSet(const std::string &name,
this->robotNodes = robotNodes;
this->kinematicRoot = kinematicRoot;
this->tcp = tcp;
+ RobotPtr rob = robot.lock();
if (!kinematicRoot && robotNodes.size()>0)
- this->kinematicRoot = robotNodes[0];
+ this->kinematicRoot = rob->getRootNode();
if (!tcp && robotNodes.size()>0)
this->tcp = robotNodes[robotNodes.size()-1];
this->robot = robot;
- RobotPtr rob = robot.lock();
- kinematicRootIsRobotRoot = false;
- if (kinematicRoot && rob && kinematicRoot == rob->getRootNode())
- kinematicRootIsRobotRoot = true;
-
// now, the objects are stored in the parent's (SceneObjectSet) data structure, so that the methods of SceneObjectSet do work
for(size_t i = 0; i < robotNodes.size(); i++)
{
@@ -48,7 +44,6 @@ RobotNodeSet::RobotNodeSet(const std::string &name,
}
}
}
-
}
@@ -138,8 +133,7 @@ RobotNodeSetPtr RobotNodeSet::createRobotNodeSet(RobotPtr robot,
RobotNodePtr kinematicRootNode = kinematicRoot;
if (!kinematicRootNode)
{
- THROW_VR_EXCEPTION_IF(robotNodes.empty(), "can not determine the root node need for creating a RobotNodeSet");
- kinematicRootNode = robotNodes[0];
+ kinematicRootNode = robot->getRootNode();
}
RobotNodePtr tcpNode = tcp;
if (!tcpNode)
@@ -293,7 +287,7 @@ void RobotNodeSet::respectJointLimits(Eigen::VectorXf &jointValues) const
robotNodes[i]->respectJointLimits(jointValues[i]);
}
}
-
+/*
void RobotNodeSet::setJointValues(const std::vector<float> &jointValues)
{
THROW_VR_EXCEPTION_IF(jointValues.size() != robotNodes.size(), "Wrong vector dimension (robotNodes:" << robotNodes.size() << ", jointValues: " << jointValues.size() << ")" << endl);
@@ -313,9 +307,9 @@ void RobotNodeSet::setJointValues(const std::vector<float> &jointValues)
else
VR_WARNING << "Robot of RNS " << name << " has been deleted! Could not set joint values!" << endl;
}
-}
-
+}*/
+/*
void RobotNodeSet::setJointValues(const Eigen::VectorXf &jointValues)
{
for (unsigned int i=0;i<robotNodes.size();i++)
@@ -332,8 +326,8 @@ void RobotNodeSet::setJointValues(const Eigen::VectorXf &jointValues)
else
VR_WARNING << "Robot of RNS " << name << " has been deleted! Could not set joint values!" << endl;
}
-}
-
+}*/
+/*
void RobotNodeSet::setJointValues( const RobotConfigPtr jointValues )
{
THROW_VR_EXCEPTION_IF(!jointValues,"NULL data");
@@ -353,7 +347,7 @@ void RobotNodeSet::setJointValues( const RobotConfigPtr jointValues )
else
VR_WARNING << "Robot of RNS " << name << " has been deleted! Could not set joint values!" << endl;
}
-}
+}*/
diff --git a/VirtualRobot/RobotNodeSet.h b/VirtualRobot/RobotNodeSet.h
index 5196686f4404f83bb689148b882df0ad8b2a1b43..551ee22485de3e8eca2a3fb3c56820306095c80b 100644
--- a/VirtualRobot/RobotNodeSet.h
+++ b/VirtualRobot/RobotNodeSet.h
@@ -104,13 +104,13 @@ public:
bool checkJointLimits(std::vector<float> &jointValues, bool verbose = false) const;
bool checkJointLimits(Eigen::VectorXf &jointValues, bool verbose = false) const;
- void setJointValues(const std::vector<float> &jointValues);
- void setJointValues(const Eigen::VectorXf &jointValues);
+ //void setJointValues(const std::vector<float> &jointValues);
+ //void setJointValues(const Eigen::VectorXf &jointValues);
/*!
Set joints that are within the given RobotConfig. Joints of this NodeSet that are not stored in jointValues remain untouched.
*/
- void setJointValues(const RobotConfigPtr jointValues);
+ //void setJointValues(const RobotConfigPtr jointValues);
RobotNodePtr& operator[](int i);
@@ -198,7 +198,7 @@ protected:
RobotNodePtr kinematicRoot;
RobotNodePtr tcp;
- bool kinematicRootIsRobotRoot;
+ //bool kinematicRootIsRobotRoot;
};
} // namespace VirtualRobot
diff --git a/VirtualRobot/RuntimeEnvironment.cpp b/VirtualRobot/RuntimeEnvironment.cpp
index 3f87fa7a365bb9c38177ce08516066fb665cff64..e561c12cbfb1c879d3ef40f03ad142c5138dcbce 100644
--- a/VirtualRobot/RuntimeEnvironment.cpp
+++ b/VirtualRobot/RuntimeEnvironment.cpp
@@ -6,6 +6,7 @@
#include "RuntimeEnvironment.h"
#include "VirtualRobotException.h"
+#include "Visualization/VisualizationFactory.h"
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <boost/algorithm/string.hpp>
@@ -318,6 +319,13 @@ namespace VirtualRobot
return standardValue;
}
+ void RuntimeEnvironment::cleanup()
+ {
+ VisualizationFactoryPtr visualizationFactory = VisualizationFactory::first(NULL);
+ if (visualizationFactory)
+ visualizationFactory->cleanup();
+ }
+
diff --git a/VirtualRobot/RuntimeEnvironment.h b/VirtualRobot/RuntimeEnvironment.h
index de17d4bcba5307ce250569a327a8be798a67bf2a..6834e38f8632b1ac2279a0fbc80489398ee09493 100644
--- a/VirtualRobot/RuntimeEnvironment.h
+++ b/VirtualRobot/RuntimeEnvironment.h
@@ -119,6 +119,11 @@ public:
//! Print status
static void print();
+
+ /*!
+ Free all resources. Usually not not needed, since on application exit all resources are freed automatically.
+ */
+ static void cleanup();
protected:
RuntimeEnvironment(){}
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index 07a52f72fe2cd96e5ecb6e76113c85e2cdbe242a..f59e6af47109e0d592ba301e41068a1bc5316497 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -37,15 +37,8 @@ SceneObject::SceneObject( const std::string &name, VisualizationNodePtr visualiz
SceneObject::~SceneObject()
{
- reset();
}
-void SceneObject::reset()
-{
- name = "not set";
- globalPose = Eigen::Matrix4f::Identity();
- initialized = false;
-}
bool SceneObject::initialize()
{
@@ -381,7 +374,7 @@ void SceneObject::print( bool printDecoration /*= true*/ )
cout << endl;
}
-void SceneObject::showBoundingBox( bool enable )
+void SceneObject::showBoundingBox( bool enable, bool wireframe )
{
if (!enable && !visualizationModel)
return; // nothing to do
@@ -406,7 +399,7 @@ void SceneObject::showBoundingBox( bool enable )
if (collisionModel)
{
BoundingBox bbox = collisionModel->getBoundingBox(false);
- VisualizationNodePtr visualizationNode = visualizationFactory->createBoundingBox(bbox);
+ VisualizationNodePtr visualizationNode = visualizationFactory->createBoundingBox(bbox, wireframe);
visualizationModel->attachVisualization("BoundingBox",visualizationNode);
}
}
@@ -492,4 +485,15 @@ std::string SceneObject::getSceneObjectXMLString(const std::string &basePath, in
return ss.str();
}
+
+void SceneObject::setMass( float m )
+{
+ physics.massKg = m;
+}
+
+void SceneObject::setInertiaMatrix( const Eigen::Matrix3f &im )
+{
+ physics.intertiaMatrix = im;
+}
+
} // namespace
diff --git a/VirtualRobot/SceneObject.h b/VirtualRobot/SceneObject.h
index 0079fb74052989f644a435cfe147ffcb74b1f429..ba48e76b26365d4abd771f53d750c5bf19da4c4c 100644
--- a/VirtualRobot/SceneObject.h
+++ b/VirtualRobot/SceneObject.h
@@ -141,7 +141,6 @@ public:
virtual VisualizationNodePtr getVisualization(SceneObject::VisualizationType visuType = SceneObject::Full);
virtual bool initialize();
- virtual void reset();
/*!
Enables/Disables the visualization updates of collision model and visualization model.
@@ -179,8 +178,9 @@ public:
If the object does not own a visualization yet, the VisualizationFactory is queried to get the first registered
VisualizationType in order to build a valid visualization.
\p enable Show or hide bounding box
+ \p wireframe Wireframe or solid visualization.
*/
- virtual void showBoundingBox( bool enable );
+ virtual void showBoundingBox( bool enable, bool wireframe = false );
@@ -244,11 +244,14 @@ public:
*/
float getMass();
+ void setMass(float m);
+
/*
Inertia matrix in kg*m^2.
*/
Eigen::Matrix3f getInertiaMatrix();
+ void setInertiaMatrix(const Eigen::Matrix3f &im);
virtual void print(bool printDecoration = true);
diff --git a/VirtualRobot/Trajectory.cpp b/VirtualRobot/Trajectory.cpp
index 2a6562cbc487d337fa983c1ab7b1662a2493f0f8..0460d07a596503f28839a2d25fa2b241bd69989b 100644
--- a/VirtualRobot/Trajectory.cpp
+++ b/VirtualRobot/Trajectory.cpp
@@ -301,6 +301,9 @@ std::vector<Eigen::Matrix4f > Trajectory::createWorkspaceTrajectory( VirtualRobo
r = rns->getTCP();
VR_ASSERT(r);
+ RobotPtr robot = rns->getRobot();
+ VR_ASSERT(robot);
+
Eigen::VectorXf jv;
rns->getJointValues(jv);
@@ -309,11 +312,11 @@ std::vector<Eigen::Matrix4f > Trajectory::createWorkspaceTrajectory( VirtualRobo
for (size_t i = 0; i < path.size(); i++)
{
// get tcp coords:
- rns->setJointValues(path[i]);
+ robot->setJointValues(rns,path[i]);
Eigen::Matrix4f m;
result.push_back(r->getGlobalPose());
}
- rns->setJointValues(jv);
+ robot->setJointValues(rns,jv);
return result;
}
@@ -381,14 +384,15 @@ std::string Trajectory::getRobotName() const
{
return rns->getRobot()->getName();
}
-
+/*
void Trajectory::apply( float t )
{
if (!rns)
return;
+ RobotPtr robot = rns->getRobot();
Eigen::VectorXf c;
interpolate(t,c);
- rns->setJointValues(c);
+ robot->setJointValues(rns,c);
}
-
+*/
}
diff --git a/VirtualRobot/Trajectory.h b/VirtualRobot/Trajectory.h
index 1e8b3a6b3f871188d8f6f60d683b780f5879da2f..5050fe16a6782da89ed3bcab41455277c5eaf41b 100644
--- a/VirtualRobot/Trajectory.h
+++ b/VirtualRobot/Trajectory.h
@@ -123,12 +123,8 @@ public:
*/
virtual void interpolate(float t, Eigen::VectorXf &storePos, int *storeIndex = NULL) const;
- /*!
- Interpolates and applies the resulting configuration at position t (0<=t<=1) to corresponding RobotNodeSet.
- */
- virtual void apply(float t);
+
/*!
-
*/
unsigned int getDimension() const;
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index 9410554fe90e8adb7d7c5abab16735b33ce9a20d..a6d2f77e5faec499ce2c748cf631631df444f284 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -127,7 +127,7 @@ VisualizationNodePtr CoinVisualizationFactory::getVisualizationFromString(const
/**
* This method reads the data from the given \p soInput and creates a new CoinVisualizationNode
- * with the read Coin model if no error occured during reading the model.
+ * with the read Coin model if no error occurred during reading the model.
* The newly created CoinVisualizationNode is then stored in \p visualisationNode.
*
* \param soInput SoInput instance from which the model is read.
@@ -221,7 +221,7 @@ std::string CoinVisualizationFactory::getName() {return "inventor";}
*/
boost::shared_ptr<VisualizationFactory> CoinVisualizationFactory::createInstance(void*)
{
- if (!SoDB::isInitialized())
+ if (!SoDB::isInitialized())
SoDB::init();
boost::shared_ptr<CoinVisualizationFactory> coinFactory(new CoinVisualizationFactory());
return coinFactory;
@@ -1604,8 +1604,7 @@ SoNode * CoinVisualizationFactory::getCoinVisualization( TrajectoryPtr t, Color
res->addChild(sep);
-
- rns->setJointValues(c);
+ rns->getRobot()->setJointValues(rns,c);
res->unrefNoDelete();
return res;
@@ -1941,6 +1940,12 @@ VirtualRobot::VisualizationNodePtr CoinVisualizationFactory::createUnitedVisuali
return result;
}
+void CoinVisualizationFactory::cleanup()
+{
+ if (SoDB::isInitialized())
+ SoDB::finish();
+}
+
} // namespace VirtualRobot
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
index 240da377510cc8e647e3098bfaea77e8917dbf10..2a35903b78366072865894316997c40ae4a1b6d1 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
@@ -212,6 +212,12 @@ public:
\see createOffscreenRenderer
*/
static bool renderOffscreen( SoOffscreenRenderer* renderer, SoCamera* cam, SoNode* scene, unsigned char **buffer);
+
+ /*!
+ Here, a manual cleanup can be called, no Coin3D access possible after this.
+ Usually no need to call cleanup explicitly, since cleanup is performed automatically at application exit.
+ */
+ virtual void cleanup();
protected:
static void GetVisualizationFromSoInput(SoInput& soInput, VisualizationNodePtr& visualizationNode, bool bbox = false);
diff --git a/VirtualRobot/Visualization/VisualizationFactory.h b/VirtualRobot/Visualization/VisualizationFactory.h
index bbdd687dac25e3e433a71d7dd498c8f58a9e0dd1..3bed0e11ce6b6b0cc4796404a148eb0d2c5541e3 100644
--- a/VirtualRobot/Visualization/VisualizationFactory.h
+++ b/VirtualRobot/Visualization/VisualizationFactory.h
@@ -82,6 +82,12 @@ public:
*/
virtual VisualizationNodePtr createUnitedVisualization(const std::vector<VisualizationNodePtr> &visualizations) const {return VisualizationNodePtr();}
+ /*!
+ Here, a manual cleanup can be called, no Coin3D access possible after this.
+ Usually no need to call cleanup explicitly, since cleanup is performed automatically at application exit.
+ */
+ virtual void cleanup(){;}
+
};
typedef boost::shared_ptr<VisualizationFactory::Color> ColorPtr;
diff --git a/VirtualRobot/Workspace/Reachability.cpp b/VirtualRobot/Workspace/Reachability.cpp
index 88c4f43a4375bfa6ef34c55deacd345d38f3e1ec..bcfaa2e2f9c67f14ee5ed3a9321c2a9e44110313 100644
--- a/VirtualRobot/Workspace/Reachability.cpp
+++ b/VirtualRobot/Workspace/Reachability.cpp
@@ -68,7 +68,7 @@ void Reachability::addRandomTCPPoses( unsigned int loops, bool checkForSelfColli
VR_WARNING << "Could not find collision-free configuration...";
}
robot->setUpdateVisualization(visuSate);
- nodeSet->setJointValues(c);
+ robot->setJointValues(nodeSet,c);
}
bool Reachability::isReachable( const Eigen::Matrix4f &globalPose )
{
diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
index bb802ee9b902cf92327d9556b117528a4c309950..0ebd88988fed40dece277cee496c4d7b949b8b4b 100644
--- a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
+++ b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
@@ -677,7 +677,7 @@ bool WorkspaceRepresentation::setRobotNodesToRandomConfig( VirtualRobot::RobotNo
maxJ = (*nodeSet)[i]->getJointLimitHi();
v[i] = minJ + ((maxJ - minJ)*rndValue);
}
- nodeSet->setJointValues(v);
+ robot->setJointValues(nodeSet,v);
// check for collisions
if (!checkForSelfCollisions || !staticCollisionModel || !dynamicCollisionModel)
return true;
@@ -1071,7 +1071,7 @@ bool WorkspaceRepresentation::checkForParameters( RobotNodeSetPtr nodeSet, float
storeMaxBounds[i] = x[i];
}
}
- nodeSet->setJointValues(c);
+ robot->setJointValues(nodeSet,c);
robot->setUpdateVisualization(visuSate);
diff --git a/VirtualRobot/XML/SceneIO.cpp b/VirtualRobot/XML/SceneIO.cpp
index 8e19ac2a43906ae2f8481c318a11de19082fc6dd..3094814095af00681cf77154438ab14657ec987f 100644
--- a/VirtualRobot/XML/SceneIO.cpp
+++ b/VirtualRobot/XML/SceneIO.cpp
@@ -131,7 +131,7 @@ bool SceneIO::processSceneRobot(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr
if (!initConfigName.empty())
{
THROW_VR_EXCEPTION_IF(!scene->hasRobotConfig(robot,initConfigName), "Scene's robot tag '" << robotName << "' does not have the initConfig '" << initConfigName << "'." << endl);
- scene->getRobotConfig(robot,initConfigName)->setJointValues();
+ robot->setJointValues(scene->getRobotConfig(robot,initConfigName));
}
return true;
}
diff --git a/VirtualRobot/examples/Jacobi/JacobiWindow.cpp b/VirtualRobot/examples/Jacobi/JacobiWindow.cpp
index 4d406d9225bc846f1e5bf4a457accbbe493b3e6f..5637d329f18c748aeba08e149f58ec58b5f0e27b 100644
--- a/VirtualRobot/examples/Jacobi/JacobiWindow.cpp
+++ b/VirtualRobot/examples/Jacobi/JacobiWindow.cpp
@@ -232,11 +232,9 @@ void JacobiWindow::resetSceneryAll()
std::vector<RobotNodePtr> rn;
robot->getRobotNodes(rn);
+ std::vector<float> jv(rn.size(),0.0f);
+ robot->setJointValues(rn,jv);
- for (unsigned int i=0;i<rn.size();i++)
- {
- rn[i]->setJointValue(0);
- }
exViewer->render();
}
diff --git a/VirtualRobot/examples/ReachabilityMap/ReachabilityMapScene.cpp b/VirtualRobot/examples/ReachabilityMap/ReachabilityMapScene.cpp
index 5bf95d1878056112d71bbc74f1596cf1d269da3b..2a4c5d1bdebe5ef17d8731a725c0ea1837fede49 100644
--- a/VirtualRobot/examples/ReachabilityMap/ReachabilityMapScene.cpp
+++ b/VirtualRobot/examples/ReachabilityMap/ReachabilityMapScene.cpp
@@ -37,7 +37,7 @@ int main(int argc, char *argv[])
filenameReach = "reachability/iCub_HipLeftArm.bin";
#else
std::string filenameRob("robots/ArmarIII/ArmarIII.xml");
- std::string fileObj("objects/ArmarIII/WaterBottel_RightHand_1000.xml");
+ std::string fileObj("objects/ArmarIII/WaterBottle_RightHand_1000.xml");
filenameReach = "reachability/ArmarIII_PlatformHipRightArm.bin";
eef = "Hand R";
#endif
diff --git a/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp b/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp
index 8e52aa4f8ec5a9fbb41edb4ca333307c752d22a1..ffe2bfddfe7ddbc242264c9c34abe3b44ad15e02 100644
--- a/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp
+++ b/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp
@@ -146,12 +146,8 @@ void ReachabilityMapWindow::resetSceneryAll()
return;
std::vector< RobotNodePtr > nodes;
robot->getRobotNodes(nodes);
-
- for (unsigned int i=0;i<nodes.size();i++)
- {
- nodes[i]->setJointValue(0);
- }
- robot->applyJointValues();
+ std::vector<float> jv(nodes.size(),0.0f);
+ robot->setJointValues(nodes,jv);
}
void ReachabilityMapWindow::updateVisu()
diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
index 90ce0eb055fa7a0b02957a099b598e56845e515c..0403aeab30169f22e6e67aa40791026e1f271ae9 100644
--- a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
+++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
@@ -149,11 +149,8 @@ void showRobotWindow::resetSceneryAll()
{
if (!robot)
return;
-
- for (unsigned int i=0;i<allRobotNodes.size();i++)
- {
- allRobotNodes[i]->setJointValue(0);
- }
+ std::vector<float> jv(allRobotNodes.size(),0.0f);
+ robot->setJointValues(allRobotNodes,jv);
selectJoint(UI.comboBoxJoint->currentIndex());
}
@@ -363,7 +360,7 @@ void showRobotWindow::jointValueChanged(int pos)
if (nr<0 || nr>=(int)currentRobotNodes.size())
return;
float fPos = currentRobotNodes[nr]->getJointLimitLo() + (float)pos / 1000.0f * (currentRobotNodes[nr]->getJointLimitHi() - currentRobotNodes[nr]->getJointLimitLo());
- currentRobotNodes[nr]->setJointValue(fPos);
+ robot->setJointValue(currentRobotNodes[nr],fPos);
UI.lcdNumberJointValue->display((double)fPos);
}
diff --git a/VirtualRobot/examples/SceneViewer/showSceneWindow.cpp b/VirtualRobot/examples/SceneViewer/showSceneWindow.cpp
index b49461f944b81cff4903a5806ab7d8aa3c62983b..ddcdbd1bb75cf830845dc3a17c72f941ceecfb20 100644
--- a/VirtualRobot/examples/SceneViewer/showSceneWindow.cpp
+++ b/VirtualRobot/examples/SceneViewer/showSceneWindow.cpp
@@ -163,7 +163,8 @@ void showSceneWindow::sliderMoved(int pos)
return;
float fpos = (float)pos / 999.0f;
- currentTrajectory->apply(fpos);
+ if (currentRobot)
+ currentRobot->setJointValues(currentTrajectory,fpos);
}
@@ -298,7 +299,7 @@ void showSceneWindow::selectRobotConfig(int nr)
VirtualRobot::RobotConfigPtr rc = scene->getRobotConfig(currentRobot->getName(), UI.comboBoxRobotConfig->currentText().toStdString());
if (!rc)
return;
- rc->setJointValues();
+ currentRobot->setJointValues(rc);
}
void showSceneWindow::selectTrajectory(int nr)
diff --git a/VirtualRobot/examples/reachability/reachabilityWindow.cpp b/VirtualRobot/examples/reachability/reachabilityWindow.cpp
index 76b11cd084daaf81f9fdcdd6d9a474d4734d91a0..a4cf26b28d1bcd7d800b61eddd099b3db9947a86 100644
--- a/VirtualRobot/examples/reachability/reachabilityWindow.cpp
+++ b/VirtualRobot/examples/reachability/reachabilityWindow.cpp
@@ -124,12 +124,8 @@ void reachabilityWindow::resetSceneryAll()
return;
std::vector< RobotNodePtr > nodes;
robot->getRobotNodes(nodes);
-
- for (unsigned int i=0;i<nodes.size();i++)
- {
- nodes[i]->setJointValue(0);
- }
- robot->applyJointValues();
+ std::vector<float> jv(nodes.size(),0.0f);
+ robot->setJointValues(nodes,jv);
}
@@ -255,7 +251,7 @@ void reachabilityWindow::jointValueChanged(int pos)
if (nr<0 || nr>=(int)allRobotNodes.size())
return;
float fPos = allRobotNodes[nr]->getJointLimitLo() + (float)pos / 1000.0f * (allRobotNodes[nr]->getJointLimitHi() - allRobotNodes[nr]->getJointLimitLo());
- allRobotNodes[nr]->setJointValue(fPos);
+ robot->setJointValue(allRobotNodes[nr],fPos);
UI.lcdNumberJointValue->display((double)fPos);
}
diff --git a/VirtualRobot/examples/stability/stabilityDemo.cpp b/VirtualRobot/examples/stability/stabilityDemo.cpp
index d509de5b251e7fe1d666fd32e5d77f2a0b51a725..e79a09af27b5c239d78362129cf74843aeb76721 100644
--- a/VirtualRobot/examples/stability/stabilityDemo.cpp
+++ b/VirtualRobot/examples/stability/stabilityDemo.cpp
@@ -8,7 +8,7 @@ int main(int argc, char *argv[])
SoDB::init();
SoQt::init(argc,argv,"stability demo");
cout << " --- START --- " << endl;
- std::string filenameRob("data/robots/ArmarIII/ArmarIII.xml");
+ std::string filenameRob("robots/ArmarIII/ArmarIII.xml");
VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(filenameRob);
//std::string filename("C:/Projects/IIT_Projects/iCubRobot/robot/iCub.xml");
diff --git a/VirtualRobot/examples/stability/stabilityWindow.cpp b/VirtualRobot/examples/stability/stabilityWindow.cpp
index 0eeb08bbd1af28ac3d717d0a94e7c548e302bc65..664a7bb23da9eaae9a8747998c4dcb3aca7cd09f 100644
--- a/VirtualRobot/examples/stability/stabilityWindow.cpp
+++ b/VirtualRobot/examples/stability/stabilityWindow.cpp
@@ -130,12 +130,8 @@ void stabilityWindow::resetSceneryAll()
return;
std::vector< RobotNodePtr > nodes;
robot->getRobotNodes(nodes);
-
- for (unsigned int i=0;i<nodes.size();i++)
- {
- nodes[i]->setJointValue(0);
- }
- robot->applyJointValues();
+ std::vector<float> jv(nodes.size(),0.0f);
+ robot->setJointValues(nodes,jv);
}
@@ -402,7 +398,7 @@ void stabilityWindow::jointValueChanged(int pos)
if (nr<0 || nr>=(int)currentRobotNodes.size())
return;
float fPos = currentRobotNodes[nr]->getJointLimitLo() + (float)pos / 1000.0f * (currentRobotNodes[nr]->getJointLimitHi() - currentRobotNodes[nr]->getJointLimitLo());
- currentRobotNodes[nr]->setJointValue(fPos);
+ robot->setJointValue(currentRobotNodes[nr],fPos);
UI.lcdNumberJointValue->display((double)fPos);
updateCoM();
@@ -418,7 +414,7 @@ void stabilityWindow::jointValueChanged(int pos)
{
for (unsigned int i=0;i<currentRobotNodeSet->getSize();i++)
{
- currentRobotNodeSet->getNode(i)->showBoundingBox(true);
+ currentRobotNodeSet->getNode(i)->showBoundingBox(true,true);
}
}
}
diff --git a/VirtualRobot/tests/VirtualRobotJacobianTest.cpp b/VirtualRobot/tests/VirtualRobotJacobianTest.cpp
index 7090d60f0ac2e6c1489b168f2b8b017e720fd12a..5795bd23d5ee1f4bd8a6aa8e1be7f9693b92f88e 100644
--- a/VirtualRobot/tests/VirtualRobotJacobianTest.cpp
+++ b/VirtualRobot/tests/VirtualRobotJacobianTest.cpp
@@ -90,7 +90,7 @@ BOOST_AUTO_TEST_CASE(testJacobianRevoluteJoint)
VirtualRobot::DifferentialIK ik(kc);
Eigen::VectorXf jV(3);
jV << 0.78f, 0.78f,0;
- kc->setJointValues(jV);
+ rob->setJointValues(kc,jV);
// Calculate the Jacobi matrix at the given position
Eigen::MatrixXf jacobian = ik.getJacobianMatrix();
@@ -99,10 +99,10 @@ BOOST_AUTO_TEST_CASE(testJacobianRevoluteJoint)
Eigen::Matrix4f a=r3->getGlobalPose();
Eigen::MatrixXf DiffQuot(3,2);
jV << 0.78f+STEP_SIZE, 0.78f,0 ;
- kc->setJointValues(jV);
+ rob->setJointValues(kc,jV);
DiffQuot.block<3,1>(0,0) = ( r3->getGlobalPose().block<3,1>(0,3) - a.block<3,1>(0,3) )/STEP_SIZE;
jV << 0.78f, 0.78f+STEP_SIZE,0;
- kc->setJointValues(jV);
+ rob->setJointValues(kc,jV);
DiffQuot.block<3,1>(0,1) = ( r3->getGlobalPose().block<3,1>(0,3) - a.block<3,1>(0,3) )/STEP_SIZE;
// Compare both and check if they are similar enough.
diff --git a/VirtualRobot/tests/VirtualRobotThreadsafetyTest.cpp b/VirtualRobot/tests/VirtualRobotThreadsafetyTest.cpp
index 544af0b8cebae5c9750a23249c7353aed93a8fa4..966f37c450a29e137dd9b7214835877da0bd0888 100644
--- a/VirtualRobot/tests/VirtualRobotThreadsafetyTest.cpp
+++ b/VirtualRobot/tests/VirtualRobotThreadsafetyTest.cpp
@@ -31,7 +31,7 @@ VirtualRobot::RobotNodePtr r2;
void thread1(){
for (int i=0; i < 20;i++){
float angle = float(i) / 100.0f * 90.0f - 45.0f;
- r1->setJointValue(angle, false);
+ rob->setJointValue(r1, angle);
}
}
diff --git a/VirtualRobot/tests/VirtualRobotTransformationTest.cpp b/VirtualRobot/tests/VirtualRobotTransformationTest.cpp
index b93ba213d3297e3fd8242b809530c1ffa050a382..0c0867894d61be4fd32752ea200180a6a356fc6f 100644
--- a/VirtualRobot/tests/VirtualRobotTransformationTest.cpp
+++ b/VirtualRobot/tests/VirtualRobotTransformationTest.cpp
@@ -85,7 +85,7 @@ BOOST_AUTO_TEST_CASE(testRobotNodePrismaticTransformation)
CHECK_TRANSFORMATION_MATRIX(m2,100.0f,200.0f,0);
// play around with joint values
- r1->setJointValue(150.0f);
+ rob->setJointValue(r1,150.0f);
m1 = r1->getGlobalPose();
m2 = r2->getGlobalPose();
@@ -94,7 +94,7 @@ BOOST_AUTO_TEST_CASE(testRobotNodePrismaticTransformation)
CHECK_TRANSFORMATION_MATRIX(m2,100.0f,200.0f,150.0f);
// play around with joint values
- r1->setJointValue(-5000.0f);
+ rob->setJointValue(r1,-5000.0f);
m1 = r1->getGlobalPose();
m2 = r2->getGlobalPose();