diff --git a/VirtualRobot/MathTools.cpp b/VirtualRobot/MathTools.cpp
index 8d0437c47939b1386c06ed2131499fc9e01ad1ef..dfdbd44b39046cc34fb55b6c1c210b5093d399a2 100644
--- a/VirtualRobot/MathTools.cpp
+++ b/VirtualRobot/MathTools.cpp
@@ -862,6 +862,49 @@ MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::axisangle2quat( con
return eigen4f2quat(m);
}
+float MathTools::getDot( const Quaternion &q1, const Quaternion &q2 )
+{
+ return q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w;
+}
+
+MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::getMean( std::vector<MathTools::Quaternion> quaternions )
+{
+ Quaternion res;
+ if (quaternions.size()==0)
+ return res;
+ res.x = res.y = res.z = res.w = 0;
+ for (size_t i=0;i<quaternions.size();i++)
+ {
+ if (getDot(res,quaternions[i])>0)
+ {
+ res.x += quaternions[i].x;
+ res.y += quaternions[i].y;
+ res.z += quaternions[i].z;
+ res.w += quaternions[i].w;
+ } else
+ {
+ res.x += -quaternions[i].x;
+ res.y += -quaternions[i].y;
+ res.z += -quaternions[i].z;
+ res.w += -quaternions[i].w;
+ }
+ }
+ float mag = sqrtf(res.x * res.x + res.y * res.y + res.z * res.z + res.w * res.w);
+
+ if (mag > 0.0001)
+ {
+ res.x /= mag;
+ res.y /= mag;
+ res.z /= mag;
+ res.w /= mag;
+ }
+ else
+ res = quaternions[0];
+ return res;
+}
+
+
+
diff --git a/VirtualRobot/MathTools.h b/VirtualRobot/MathTools.h
index 90b3204b0d9e49482a0b63a077c661bfed2bc057..a2c34b1b350818ba4de6874cee21bb2a97243bb7 100644
--- a/VirtualRobot/MathTools.h
+++ b/VirtualRobot/MathTools.h
@@ -122,6 +122,12 @@ namespace MathTools
//! Returns q1*q2
Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT multiplyQuaternions( const Quaternion &q1, const Quaternion &q2 );
+ //! returns q1 dot q2
+ float VIRTUAL_ROBOT_IMPORT_EXPORT getDot(const Quaternion &q1, const Quaternion &q2 );
+
+ //! Computes mean orientation of quaternions
+ MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT getMean(std::vector<MathTools::Quaternion> quaternions );
+
//! Get the projected point in 2D (local coordinate system of the plane)
@@ -245,6 +251,8 @@ namespace MathTools
void VIRTUAL_ROBOT_IMPORT_EXPORT convertMM2M( const std::vector<ContactPoint> points, std::vector<ContactPoint> &storeResult );
bool VIRTUAL_ROBOT_IMPORT_EXPORT ensureOrthonormalBasis(Eigen::Vector3f &x, Eigen::Vector3f &y, Eigen::Vector3f &z);
+
+
};
} // namespace VirtualRobot
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index 71811065a16aa7a4c28763c3dfc7d615d990981d..a9bfa5bad573437cf044a7e98c77c37f831afb08 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -598,6 +598,26 @@ void Robot::createVisualizationFromCollisionModels()
}
}
+VirtualRobot::RobotConfigPtr Robot::getConfig()
+{
+ RobotConfigPtr r(new RobotConfig(shared_from_this(),getName()));
+ std::map< std::string, RobotNodePtr >::iterator i = robotNodeMap.begin();
+ while (i!=robotNodeMap.end())
+ {
+ RobotNodePtr rn = i->second;
+ if (rn->isTranslationalJoint() || rn->isRotationalJoint())
+ r->setConfig(rn,rn->getJointValue());
+ i++;
+ }
+ return r;
+}
+
+bool Robot::setConfig( RobotConfigPtr c )
+{
+ if (c)
+ return c->applyToRobot(shared_from_this());
+ return false;
+}
} // namespace VirtualRobot
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index 4fcf572861a5f85950a5fe5c16f07e523aaf2802..85bd3e1284f8af1a3ebb6a3dfe2306de2af523f0 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -26,6 +26,7 @@
#include "VirtualRobotImportExport.h"
#include "Nodes/RobotNode.h"
#include "RobotNodeSet.h"
+#include "RobotConfig.h"
#include <boost/enable_shared_from_this.hpp>
#include <boost/type_traits/is_base_of.hpp>
@@ -114,6 +115,14 @@ public:
void setUpdateVisualization (bool enable);
bool getUpdateVisualizationStatus();
+ /*!
+ get the complete setup of all robot nodes
+ */
+ RobotConfigPtr getConfig();
+ /*!
+ Sets the configuration according to the RobtoNodes, defined in c. All other nodes are not affected.
+ */
+ bool setConfig(RobotConfigPtr c);
/*!
This method is automatically called in RobotNode's initialization routine.
diff --git a/VirtualRobot/VirtualRobot.h b/VirtualRobot/VirtualRobot.h
index 39eb0135e3ebf380cc82330b602b3a0cfe5c6935..9cb2e1b4bf5fc863ddd4b839dc6473b51a13d87e 100644
--- a/VirtualRobot/VirtualRobot.h
+++ b/VirtualRobot/VirtualRobot.h
@@ -31,42 +31,60 @@
-/** \mainpage Simox: A simulation, motion and grasp plannign toolbox.
- *
- *
- * \section Introduction
- *
- *
- * The aim of the lightweight platform independent C++ toolbox Simox is to provide a set of
- * algorithms for 3D simulation of robot systems, sampling based motion planning and grasp
- * planning. Simox consists of three libraries (VirtualRobot, Saba and GraspStudio) and numerous
- * examples showing how these libraries can be used to build complex tools in the
- * context of mobile manipulation. The library VirtualRobot can be used to define complex
- * robot systems, which may cover multiple robots with many degrees of freedom. The robot
- * structure and its visualization can be easily defined via XML files and environments with
- * obstacles and objects to manipulate are supported. Further, basic robot simulation components,
- * as Jacobian computations and generic Inverse Kinematics (IK) solvers, are offered by
- * the library. Beyond that, extended features like tools for analyzing the reachable workspace
- * for robotic manipulators or contact determination for grasping are included.
- * With Saba, a library for planning collision-free motions is offered, which directly incorporates
- * with the data provided by VirtualRobot. The algorithms cover state-of-the-art implementations
- * of sampling-based motion planning approaches (e.g. Rapidly-exploring Random Trees)
- * and interfaces that allow to conveniently implement own planners. Since Saba was designed
- * for planning in high-dimensional configuration spaces, complex planning problems for robots
- * with a high number of degrees of freedom (DoF) can be solved efficiently.
- * GraspStudio offers possibilities to compute the grasp quality for generic end-effector definitions,
- * e.g. a humanoid hand. The implemented 6D wrench-space computations can be used
- * to easily (and quickly) determine the quality of an applied grasp to an object. Furthermore,
- * the implemented planners are able to generate grasp maps for given objects automatically.
- * Since complex frameworks have to incorporate with several libraries in order to provide full
- * functionality, several issues may arise when setting up the environment, such as dependency
- * problems, incompatible library versions or even non-existing ports of needed libraries for the
- * used operating systems. Hence, only a limited set of libraries are used by the Simox core in
- * order to make it compile. Extended functionality (e.g. visualization) can be turned off in
- * order to allow Simox compiling on most platforms. Further dependencies are encapsulated
- * with interfaces, making it easy to exchange e.g. the collision engine or the visualization
- * functionality. As a reference implementation Simox offers Coin3D/SoQt-based visualization
- * support.
+/** \mainpage Simox: A simulation, motion and grasp planning toolbox.
+
+ \section Introduction Introduction
+
+ The aim of the lightweight platform independent C++ toolbox \b Simox is to provide a set of
+ algorithms for 3D simulation of robot systems, sampling based motion planning and grasp
+ planning. Simox consists of three libraries (VirtualRobot, Saba and GraspStudio) and numerous
+ examples showing how these libraries can be used to build complex tools in the
+ context of mobile manipulation.
+
+ \section VirtualRobot VirtualRobot
+
+ The library \b VirtualRobot can be used to define complex
+ robot systems, which may cover multiple robots with many degrees of freedom. The robot
+ structure and its visualization can be easily defined via XML files and environments with
+ obstacles and objects to manipulate are supported. Further, basic robot simulation components,
+ as Jacobian computations and generic Inverse Kinematics (IK) solvers, are offered by
+ the library. Beyond that, extended features like tools for analyzing the reachable workspace
+ for robotic manipulators or contact determination for grasping are included.
+ \image html VR.png
+
+ \section Saba Motion Planning
+
+ With \b Saba, a library for planning collision-free motions is offered, which directly incorporates
+ with the data provided by VirtualRobot. The algorithms cover state-of-the-art implementations
+ of sampling-based motion planning approaches (e.g. Rapidly-exploring Random Trees)
+ and interfaces that allow to conveniently implement own planners. Since Saba was designed
+ for planning in high-dimensional configuration spaces, complex planning problems for robots
+ with a high number of degrees of freedom (DoF) can be solved efficiently.
+
+ \image html Saba.png
+
+ \section GraspStudio Grasp Planning
+
+ \b GraspStudio offers possibilities to compute the grasp quality for generic end-effector definitions,
+ e.g. a humanoid hand. The implemented 6D wrench-space computations can be used
+ to easily (and quickly) determine the quality of an applied grasp to an object. Furthermore,
+ the implemented planners are able to generate grasp maps for given objects automatically.
+
+ \image html GraspStudio.png
+
+ \section Wiki Installation, tutorials and documentation
+
+ Since complex frameworks have to incorporate with several libraries in order to provide full
+ functionality, several issues may arise when setting up the environment, such as dependency
+ problems, incompatible library versions or even non-existing ports of needed libraries for the
+ used operating systems. Hence, only a limited set of libraries are used by the Simox core in
+ order to make it compile. Extended functionality (e.g. visualization) can be turned off in
+ order to allow Simox compiling on most platforms. Further dependencies are encapsulated
+ with interfaces, making it easy to exchange e.g. the collision engine or the visualization
+ functionality. As a reference implementation Simox offers Coin3D/SoQt-based visualization
+ support.
+
+ Please have a look at the wiki pages: http://sourceforge.net/apps/mediawiki/simox
*
*/
@@ -153,6 +171,9 @@ namespace VirtualRobot
#ifdef _DEBUG
+/*!
+ This assert macro does nothing on RELEASE builds.
+*/
#define VR_ASSERT(a) THROW_VR_EXCEPTION_IF(!(a), "ASSERT failed (" << #a << ")" );
#define VR_ASSERT_MESSAGE(a,b) THROW_VR_EXCEPTION_IF(!(a), "ASSERT failed (" << #a << "): " << b );
#else
diff --git a/VirtualRobot/XML/rapidxml_license.txt b/VirtualRobot/XML/rapidxml_license.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3ab806d0af7905bec5932fe0d0a6b253f5951204
--- /dev/null
+++ b/VirtualRobot/XML/rapidxml_license.txt
@@ -0,0 +1,52 @@
+Use of this software is granted under one of the following two licenses,
+to be chosen freely by the user.
+
+1. Boost Software License - Version 1.0 - August 17th, 2003
+===============================================================================
+
+Copyright (c) 2006, 2007 Marcin Kalicinski
+
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by
+this license (the "Software") to use, reproduce, display, distribute,
+execute, and transmit the Software, and to prepare derivative works of the
+Software, and to permit third-parties to whom the Software is furnished to
+do so, all subject to the following:
+
+The copyright notices in the Software and this entire statement, including
+the above license grant, this restriction and the following disclaimer,
+must be included in all copies of the Software, in whole or in part, and
+all derivative works of the Software, unless such copies or derivative
+works are solely in the form of machine-executable object code generated by
+a source language processor.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
+
+2. The MIT License
+===============================================================================
+
+Copyright (c) 2006, 2007 Marcin Kalicinski
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
+of the Software, and to permit persons to whom the Software is furnished to do so,
+subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+IN THE SOFTWARE.
\ No newline at end of file