diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index f09afe8aab09f035f22154b49bfd182bd360abe8..294ae06f6bca216b531b667718d03ff3f66a0895 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -333,8 +333,9 @@ SET(SOURCES
Nodes/RobotNodeRevolute.cpp
Nodes/RobotNodeRevoluteFactory.cpp
Nodes/Sensor.cpp
+ Nodes/HemisphereJoint/CachedMaths.cpp
Nodes/HemisphereJoint/Expressions.cpp
- Nodes/HemisphereJoint/Joint.cpp
+ Nodes/HemisphereJoint/Maths.cpp
TimeOptimalTrajectory/Path.cpp
TimeOptimalTrajectory/TimeOptimalTrajectory.cpp
@@ -577,8 +578,9 @@ SET(INCLUDES
Nodes/RobotNodeRevoluteFactory.h
Nodes/Sensor.h
Nodes/SensorFactory.h
+ Nodes/HemisphereJoint/CachedMaths.h
Nodes/HemisphereJoint/Expressions.h
- Nodes/HemisphereJoint/Joint.h
+ Nodes/HemisphereJoint/Maths.h
TimeOptimalTrajectory/Path.h
TimeOptimalTrajectory/TimeOptimalTrajectory.h
diff --git a/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp
index f35569f728c3412be25276ca6d6fce59da010786..70a41bfbc7707cf74781d2587e57348e0ac187a1 100644
--- a/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp
+++ b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp
@@ -206,35 +206,40 @@ void CompositeDiffIK::step(CompositeDiffIK::Parameters& params, SolveState& s, i
ik->updatePseudoInverseJacobianMatrix(s.invJac, s.jacobi, 0, s.cartesianRegularization);
+ Eigen::VectorXf jv = s.invJac * cartesianVel;
- Eigen::VectorXf nullspaceVel = Eigen::VectorXf::Zero(s.cols);
-
- for (const NullspaceGradientPtr& nsGradient : nullspaceGradients)
+ if (s.cols > s.rows)
{
- Eigen::VectorXf nsgrad = nsGradient->kP * nsGradient->getGradientAdjusted(params, stepNr);
- nullspaceVel += nsgrad;
- }
- //LimitInfNormTo(nullspaceVel, params.maxJointAngleStep);
+ Eigen::VectorXf nullspaceVel = Eigen::VectorXf::Zero(s.cols);
- Eigen::JacobiSVD<Eigen::MatrixXf> svd(s.jacobi, Eigen::ComputeFullU | Eigen::ComputeFullV);
- Eigen::MatrixXf V = svd.matrixV();
- Eigen::MatrixXf nullspaceSVD = V.block(0, s.rows, s.cols, s.cols - s.rows);
+ for (const NullspaceGradientPtr& nsGradient : nullspaceGradients)
+ {
+ Eigen::VectorXf nsgrad = nsGradient->kP * nsGradient->getGradientAdjusted(params, stepNr);
+ nullspaceVel += nsgrad;
+ }
- s.nullspace = nullspaceSVD; // CalculateNullspaceSVD(s.jacobi);
+ //LimitInfNormTo(nullspaceVel, params.maxJointAngleStep);
- Eigen::VectorXf nsv = Eigen::VectorXf::Zero(s.cols);
- for (int i = 0; i < s.nullspace.cols(); i++)
- {
- float squaredNorm = s.nullspace.col(i).squaredNorm();
- // Prevent division by zero
- if (squaredNorm > 1.0e-32f)
+ Eigen::JacobiSVD<Eigen::MatrixXf> svd(s.jacobi, Eigen::ComputeFullU | Eigen::ComputeFullV);
+ Eigen::MatrixXf V = svd.matrixV();
+
+ Eigen::MatrixXf nullspaceSVD = V.block(0, s.rows, s.cols, s.cols - s.rows);
+
+ s.nullspace = nullspaceSVD; // CalculateNullspaceSVD(s.jacobi);
+
+ Eigen::VectorXf nsv = Eigen::VectorXf::Zero(s.cols);
+ for (int i = 0; i < s.nullspace.cols(); i++)
{
- nsv += s.nullspace.col(i) * s.nullspace.col(i).dot(nullspaceVel) / s.nullspace.col(i).squaredNorm();
+ float squaredNorm = s.nullspace.col(i).squaredNorm();
+ // Prevent division by zero
+ if (squaredNorm > 1.0e-32f)
+ {
+ nsv += s.nullspace.col(i) * s.nullspace.col(i).dot(nullspaceVel) / s.nullspace.col(i).squaredNorm();
+ }
}
+ jv = jv + nsv;
}
- Eigen::VectorXf jv = s.invJac * cartesianVel;
- jv = jv + nsv;
jv = jv * params.stepSize;
jv = LimitInfNormTo(jv, params.maxJointAngleStep, params.maxJointAngleStepIgnore);
jv = jv.cwiseProduct(s.jointRegularization);
diff --git a/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h
index 71e27d22469016e0a6a246255140428d4687f6a8..595b3b51f2e8ca0e408762f7473cdb15881c35e3 100644
--- a/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h
+++ b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h
@@ -38,7 +38,7 @@ namespace VirtualRobot
// IK params
size_t steps = 40;
- float maxJointAngleStep = 0.2f;
+ float maxJointAngleStep = 0.01f;
float stepSize = 0.5f;
bool resetRnsValues = true;
bool returnIKSteps = false;
diff --git a/VirtualRobot/IK/DifferentialIK.cpp b/VirtualRobot/IK/DifferentialIK.cpp
index 39d4ee31015b87431499b640a6b6aa1cebcb1b3d..1c64868f1abd9d3e90302fff7eda7610c12533a1 100644
--- a/VirtualRobot/IK/DifferentialIK.cpp
+++ b/VirtualRobot/IK/DifferentialIK.cpp
@@ -6,6 +6,7 @@
#include "../Nodes/RobotNodeRevolute.h"
#include "../VirtualRobotException.h"
#include "../CollisionDetection/CollisionChecker.h"
+#include <VirtualRobot/Nodes/RobotNodeHemisphere.h>
#include <Eigen/Geometry>
@@ -380,9 +381,9 @@ namespace VirtualRobot
//check if the tcp is affected by this DOF
auto p = parents[tcpRN];
- if (find(p.begin(), p.end(), dof) != p.end())
+ const bool isParent = std::find(p.begin(), p.end(), dof) != p.end();
+ if (isParent)
{
-
// Calculus for rotational joints is different as for prismatic joints.
if (dof->isRotationalJoint())
{
@@ -443,6 +444,29 @@ namespace VirtualRobot
// no orientation part required with prismatic joints
}
+ else if (dof->isHemisphereJoint())
+ {
+ RobotNodeHemispherePtr hemisphere
+ = std::dynamic_pointer_cast<RobotNodeHemisphere>(dof);
+
+ VR_ASSERT(hemisphere->first.has_value() xor hemisphere->second.has_value());
+
+ if (hemisphere->isSecondHemisphereJointNode())
+ {
+ // Set Jacobian for both DoFs.
+ RobotNodeHemisphere::SecondData& second = hemisphere->getSecondData();
+ const hemisphere::Maths::Jacobian jacobian = second.getJacobian();
+
+ tmpUpdateJacobianPosition.block<3, 2>(0, i - 1)
+ = jacobian.block<3, 2>(0, 0).cast<float>();
+ tmpUpdateJacobianOrientation.block<3, 2>(0, i - 1)
+ = jacobian.block<3, 2>(3, 0).cast<float>();
+ }
+ else
+ {
+ // Nothing to do - everything is handled by second DoF.
+ }
+ }
}
#ifdef CHECK_PERFORMANCE
diff --git a/VirtualRobot/Nodes/HemisphereJoint/CachedMaths.cpp b/VirtualRobot/Nodes/HemisphereJoint/CachedMaths.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e52ac985bb26927b5dfff6ee22e9d45a67a3f7b2
--- /dev/null
+++ b/VirtualRobot/Nodes/HemisphereJoint/CachedMaths.cpp
@@ -0,0 +1,15 @@
+#include "CachedMaths.h"
+
+
+namespace VirtualRobot::hemisphere
+{
+
+ void CachedMaths::update(const Eigen::Vector2f& actuators)
+ {
+ if (actuators != this->actuators)
+ {
+ maths.computeFkOfAngle(actuators.cast<double>());
+ }
+ }
+
+}
diff --git a/VirtualRobot/Nodes/HemisphereJoint/CachedMaths.h b/VirtualRobot/Nodes/HemisphereJoint/CachedMaths.h
new file mode 100644
index 0000000000000000000000000000000000000000..9f2c139326e794a6763b99db25ccb1612e7eb0ee
--- /dev/null
+++ b/VirtualRobot/Nodes/HemisphereJoint/CachedMaths.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include "Maths.h"
+
+#include <Eigen/Core>
+
+
+namespace VirtualRobot::hemisphere
+{
+
+ /**
+ * @brief Wrapper around `hemisphere::Joint` caching the results.
+ */
+ class CachedMaths
+ {
+ public:
+
+ /**
+ * @brief Recompute the maths if the given `actuators` differ from
+ * the stored `actuators`.
+ */
+ void update(const Eigen::Vector2f& actuators);
+
+ public:
+
+ /// The actuator values that were used to compute the joint math.
+ Eigen::Vector2f actuators = Eigen::Vector2f::Constant(std::numeric_limits<float>::min());
+
+ /// The joint math.
+ hemisphere::Maths maths;
+
+ };
+
+}
diff --git a/VirtualRobot/Nodes/HemisphereJoint/Joint.cpp b/VirtualRobot/Nodes/HemisphereJoint/Joint.cpp
deleted file mode 100644
index 1d5a26ad82b6099338568961f1008b3cd415cf9d..0000000000000000000000000000000000000000
--- a/VirtualRobot/Nodes/HemisphereJoint/Joint.cpp
+++ /dev/null
@@ -1,97 +0,0 @@
-#include "Joint.h"
-
-#include <cmath>
-
-#include <SimoxUtility/math/convert/deg_to_rad.h>
-#include <SimoxUtility/math/pose/pose.h>
-
-
-namespace VirtualRobot::hemisphere
-{
-
- Joint::Joint() :
- Joint(1, simox::math::deg_to_rad(25.))
- {
- }
-
-
- Joint::Joint(double lever, double theta0)
- {
- this->setConstants(lever, theta0);
- }
-
-
- void Joint::setConstants(double lever, double theta0)
- {
- this->lever = lever;
- this->theta0 = theta0;
- this->radius = 2 * std::sin(theta0) * lever;
-
- this->limitHi = simox::math::deg_to_rad(45 - 6.0);
- this->limitLo = - simox::math::deg_to_rad(45 - 14.0);
- }
-
-
- void Joint::computeFkOfPosition(double p1, double p2)
- {
- fk.compute(p1, p2, lever, theta0);
- }
-
-
- void Joint::computeFkOfPosition(const Eigen::Vector2d& p12)
- {
- computeFkOfPosition(p12(0), p12(1));
- }
-
-
- void Joint::computeFkOfAngle(const Eigen::Vector2d& alpha12)
- {
- computeFkOfPosition(angleToPosition(alpha12));
- }
-
-
- Eigen::Vector3d Joint::getEndEffectorTranslation() const
- {
- return Eigen::Vector3d {
- fk.ex,
- fk.ey,
- fk.ez
- };
- }
-
-
- Eigen::Matrix3d Joint::getEndEffectorRotation() const
- {
- // r_wrist_to_base = np.array([[exx, eyx, ezx], [exy, eyy, ezy], [exz, eyz, ezz]])
- Eigen::Matrix3d ori;
- ori << fk.exx, fk.eyx, fk.ezx,
- fk.exy, fk.eyy, fk.ezy,
- fk.exz, fk.eyz, fk.ezz;
- return ori;
- }
-
-
- Eigen::Matrix4d Joint::getEndEffectorTransform() const
- {
- return simox::math::pose(getEndEffectorTranslation(), getEndEffectorRotation());
- }
-
-
- Joint::Jacobian Joint::getJacobian() const
- {
- Joint::Jacobian jacobian;
- jacobian << fk.jx1, fk.jx2,
- fk.jy1, fk.jy2,
- fk.jz1, fk.jz2,
- fk.jrx1, fk.jrx2,
- fk.jry1, fk.jry2,
- fk.jrz1, fk.jrz2;
- return jacobian;
- }
-
- Eigen::Vector2d Joint::angleToPosition(const Eigen::Vector2d& alpha) const
- {
- return lever * Eigen::sin((alpha + Eigen::Vector2d::Constant(theta0)).array());
- }
-
-}
diff --git a/VirtualRobot/Nodes/HemisphereJoint/Joint.h b/VirtualRobot/Nodes/HemisphereJoint/Joint.h
deleted file mode 100644
index 7d2bda4861ccf44c13f02d513a3f38378d6eafd1..0000000000000000000000000000000000000000
--- a/VirtualRobot/Nodes/HemisphereJoint/Joint.h
+++ /dev/null
@@ -1,54 +0,0 @@
-#pragma once
-
-#include <Eigen/Core>
-
-#include "Expressions.h"
-
-
-namespace VirtualRobot::hemisphere
-{
-
- class Joint
- {
- public:
-
- using Jacobian = Eigen::Matrix<double, 6, 2>;
-
- public:
-
- Joint();
- Joint(double lever, double theta0);
-
-
- void setConstants(double lever, double theta0);
-
-
- void computeFkOfAngle(const Eigen::Vector2d& alpha12);
-
- void computeFkOfPosition(const Eigen::Vector2d& p12);
- void computeFkOfPosition(double p1, double p2);
-
-
- Eigen::Vector3d getEndEffectorTranslation() const;
- Eigen::Matrix3d getEndEffectorRotation() const;
- Eigen::Matrix4d getEndEffectorTransform() const;
- Jacobian getJacobian() const;
-
- Eigen::Vector2d angleToPosition(const Eigen::Vector2d& alpha) const;
-
-
- public:
-
- double lever = 0;
- double theta0 = 0;
- double radius = 0;
-
- double limitLo = 0;
- double limitHi = 0;
-
-
- Expressions fk;
-
- };
-
-}
diff --git a/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp b/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8ace6dfe80971894e197b8133b4e5cd5e12cecb2
--- /dev/null
+++ b/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp
@@ -0,0 +1,122 @@
+#include "Maths.h"
+
+#include <cmath>
+
+#include <SimoxUtility/math/convert/deg_to_rad.h>
+#include <SimoxUtility/math/pose/pose.h>
+
+
+namespace VirtualRobot::hemisphere
+{
+
+ Maths::Maths() :
+ Maths(1, simox::math::deg_to_rad(25.))
+ {
+ }
+
+
+ Maths::Maths(double lever, double theta0)
+ {
+ this->setConstants(lever, theta0);
+ }
+
+
+ void Maths::setConstants(double lever, double theta0)
+ {
+ this->lever = lever;
+ this->theta0Rad = theta0;
+ this->radius = 2 * std::sin(theta0) * lever;
+
+ this->limitHi = simox::math::deg_to_rad(45 - 6.0);
+ this->limitLo = - simox::math::deg_to_rad(45 - 14.0);
+ }
+
+
+ void Maths::computeFkOfPosition(double p1, double p2)
+ {
+ expressions.compute(p1, p2, lever, theta0Rad);
+ }
+
+
+ void Maths::computeFkOfPosition(const ActuatorPosition& p12)
+ {
+ computeFkOfPosition(p12(0), p12(1));
+ }
+
+
+ void Maths::computeFkOfAngle(const ActuatorAngle& alpha12)
+ {
+ computeFkOfPosition(angleToPosition(alpha12));
+ }
+
+
+ Eigen::Vector3d Maths::getEndEffectorTranslation() const
+ {
+ return Eigen::Vector3d {
+ expressions.ex,
+ expressions.ey,
+ expressions.ez
+ };
+ }
+
+
+ Eigen::Matrix3d Maths::getEndEffectorRotation() const
+ {
+ // r_wrist_to_base = np.array([[exx, eyx, ezx], [exy, eyy, ezy], [exz, eyz, ezz]])
+ Eigen::Matrix3d ori;
+ ori << expressions.exx, expressions.eyx, expressions.ezx,
+ expressions.exy, expressions.eyy, expressions.ezy,
+ expressions.exz, expressions.eyz, expressions.ezz;
+ return ori;
+ }
+
+
+ Eigen::Matrix4d Maths::getEndEffectorTransform() const
+ {
+ return simox::math::pose(getEndEffectorTranslation(), getEndEffectorRotation());
+ }
+
+
+ Maths::Jacobian Maths::getJacobian() const
+ {
+ Maths::Jacobian jacobian;
+ jacobian << expressions.jx1, expressions.jx2,
+ expressions.jy1, expressions.jy2,
+ expressions.jz1, expressions.jz2,
+ expressions.jrx1, expressions.jrx2,
+ expressions.jry1, expressions.jry2,
+ expressions.jrz1, expressions.jrz2;
+
+ // Current state of constructing the orientational part.
+ // ToDo: Do this with symbolic math inside `Expressions`.
+ {
+ const Eigen::Vector3d eefStateTrans = getEndEffectorTranslation();
+
+ for (int column = 0; column < 2; ++column)
+ {
+ // Imagine we apply (+1, 0) / (0, +1) actuator velocities.
+ const Eigen::Vector3d eefVelTrans = jacobian.block<3, 1>(0, column); // * 1.0
+
+ /*
+ * The rotation axis is orthogonal to the vector from origin to the
+ * EEF (eefStateTrans) and the movement direction (eefVelTrans).
+ *
+ * For the scaling, ask Cornelius. :)
+ */
+ const Eigen::Vector3d scaledRotAxis = eefStateTrans.cross(eefVelTrans)
+ / eefStateTrans.norm() * 2;
+
+ jacobian.block<3, 1>(3, column) = scaledRotAxis; // / 1.0;
+ }
+ }
+
+ return jacobian;
+ }
+
+
+ Maths::ActuatorPosition Maths::angleToPosition(const Maths::ActuatorAngle& alpha) const
+ {
+ return lever * Eigen::sin((alpha + Eigen::Vector2d::Constant(theta0Rad)).array());
+ }
+
+}
diff --git a/VirtualRobot/Nodes/HemisphereJoint/Maths.h b/VirtualRobot/Nodes/HemisphereJoint/Maths.h
new file mode 100644
index 0000000000000000000000000000000000000000..76f0188b727a41d6a24c236b29fde0eadaf2ee18
--- /dev/null
+++ b/VirtualRobot/Nodes/HemisphereJoint/Maths.h
@@ -0,0 +1,64 @@
+#pragma once
+
+#include <Eigen/Core>
+
+#include "Expressions.h"
+
+
+namespace VirtualRobot::hemisphere
+{
+
+ /**
+ * @brief Hemisphere joint mathematics for FK and IK.
+ *
+ * This class is a wrapper about `Expressions`, which contains the actual
+ * math code that was generated from Python sympy expressions
+ * using `python/hemisphere-joint-demo/hemisphere_joint_demo/sympy_to_code.py`.
+ */
+ class Maths
+ {
+ public:
+
+ using ActuatorPosition = Eigen::Vector2d;
+ using ActuatorAngle = Eigen::Vector2d;
+ using Jacobian = Eigen::Matrix<double, 6, 2>;
+
+ public:
+
+ Maths();
+ Maths(double lever, double theta0);
+
+
+ void setConstants(double lever, double theta0);
+
+
+ void computeFkOfAngle(const ActuatorAngle& alpha12);
+
+ void computeFkOfPosition(const ActuatorPosition& p12);
+ void computeFkOfPosition(double p1, double p2);
+
+
+ Eigen::Vector3d getEndEffectorTranslation() const;
+ Eigen::Matrix3d getEndEffectorRotation() const;
+ Eigen::Matrix4d getEndEffectorTransform() const;
+
+ Jacobian getJacobian() const;
+
+ ActuatorPosition angleToPosition(const ActuatorAngle& alpha) const;
+
+
+ public:
+
+ double lever = 0;
+ double theta0Rad = 0;
+ double radius = 0;
+
+ double limitLo = 0;
+ double limitHi = 0;
+
+
+ Expressions expressions;
+
+ };
+
+}
diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
index ef49ea6008ef9715494400000ab9b31f5b87beeb..a2d999867affb1b8adf6f5d79ffc12675ef5607b 100644
--- a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
+++ b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
@@ -10,12 +10,13 @@
#include <SimoxUtility/meta/enum/EnumNames.hpp>
#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/math/convert/rad_to_deg.h>
namespace VirtualRobot
{
- static const float limit = 1.0;
+ static const float initialLimit = 1.0;
extern const simox::meta::EnumNames<RobotNodeHemisphere::Role> RoleNames =
{
@@ -35,10 +36,7 @@ namespace VirtualRobot
RobotNodeHemisphere::RobotNodeHemisphere(
RobotWeakPtr rob,
const std::string& name,
- float jointLimitLo,
- float jointLimitHi,
const Eigen::Matrix4f& preJointTransform,
- const Eigen::Vector3f& axis,
VisualizationNodePtr visualization,
CollisionModelPtr collisionModel,
float jointValueOffset,
@@ -46,12 +44,9 @@ namespace VirtualRobot
CollisionCheckerPtr colChecker,
RobotNodeType type
) :
- RobotNode(rob, name, -limit, limit, visualization, collisionModel,
+ RobotNode(rob, name, -initialLimit, initialLimit, visualization, collisionModel,
jointValueOffset, physics, colChecker, type)
{
- (void) axis;
- (void) jointLimitLo, (void) jointLimitHi;
-
initialized = false;
optionalDHParameter.isSet = false;
localTransformation = preJointTransform;
@@ -62,8 +57,6 @@ namespace VirtualRobot
RobotNodeHemisphere::RobotNodeHemisphere(
RobotWeakPtr rob,
const std::string& name,
- float jointLimitLo,
- float jointLimitHi,
float a, float d, float alpha, float theta,
VisualizationNodePtr visualization,
CollisionModelPtr collisionModel,
@@ -72,7 +65,7 @@ namespace VirtualRobot
CollisionCheckerPtr colChecker,
RobotNodeType type
) :
- RobotNode(rob, name, jointLimitLo, jointLimitHi, visualization, collisionModel,
+ RobotNode(rob, name, -initialLimit, initialLimit, visualization, collisionModel,
jointValueOffset, physics, colChecker, type)
{
initialized = false;
@@ -109,16 +102,16 @@ namespace VirtualRobot
void RobotNodeHemisphere::setXmlInfo(const XmlInfo& info)
{
- VR_ASSERT(second.has_value());
+ VR_ASSERT(secondData.has_value());
switch (info.role)
{
case Role::FIRST:
- first.emplace(First{});
- first->math.joint.setConstants(info.lever, info.theta0);
+ firstData.emplace(FirstData{});
+ firstData->maths.maths.setConstants(info.lever, info.theta0Rad);
break;
case Role::SECOND:
- second.emplace(Second{});
+ secondData.emplace(SecondData{});
break;
}
}
@@ -128,17 +121,18 @@ namespace VirtualRobot
SceneObjectPtr parent,
const std::vector<SceneObjectPtr>& children)
{
- VR_ASSERT_MESSAGE(first.has_value() xor second.has_value(), std::stringstream() << first.has_value() << " / " << second.has_value());
+ VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(),
+ std::stringstream() << firstData.has_value() << " / " << secondData.has_value());
// The second node needs to store a reference to the first node.
- if (second)
+ if (secondData)
{
- VR_ASSERT_MESSAGE(not second->first, "Second must not be initialized yet.");
+ VR_ASSERT_MESSAGE(not secondData->first, "Second must not be initialized yet.");
RobotNodeHemisphere* firstNode = dynamic_cast<RobotNodeHemisphere*>(parent.get());
RobotNodeHemisphere* secondNode = this;
- if (not (firstNode and firstNode->first))
+ if (not (firstNode and firstNode->firstData))
{
std::stringstream ss;
ss << "The parent of a hemisphere joint with role '" << RoleNames.to_name(Role::SECOND) << "' "
@@ -147,17 +141,18 @@ namespace VirtualRobot
}
// Save pointer to firstNode
- second->first = firstNode;
+ secondData->firstNode = firstNode;
+ secondData->secondNode = secondNode;
// Set up robot node parameters.
{
- const hemisphere::Joint& joint = second->math().joint;
+ const hemisphere::Maths& maths = secondData->maths().maths;
- firstNode->jointLimitLo = joint.limitLo;
- secondNode->jointLimitLo = joint.limitLo;
+ firstNode->jointLimitLo = maths.limitLo;
+ secondNode->jointLimitLo = maths.limitLo;
- firstNode->jointLimitHi = joint.limitHi;
- secondNode->jointLimitHi = joint.limitHi;
+ firstNode->jointLimitHi = maths.limitHi;
+ secondNode->jointLimitHi = maths.limitHi;
}
}
@@ -165,49 +160,42 @@ namespace VirtualRobot
}
- void RobotNodeHemisphere::JointMath::update(const Eigen::Vector2f& actuators)
- {
- if (actuators != this->actuators)
- {
- joint.computeFkOfAngle(actuators.cast<double>());
- }
- }
-
-
void RobotNodeHemisphere::updateTransformationMatrices(
const Eigen::Matrix4f& parentPose)
{
- VR_ASSERT_MESSAGE(first.has_value() xor second.has_value(), std::stringstream() << first.has_value() << " / " << second.has_value());
+ VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(), std::stringstream() << firstData.has_value() << " / " << secondData.has_value());
- if (first)
+ if (firstData)
{
globalPose = parentPose * localTransformation;
}
- else if (second)
+ else if (secondData)
{
- VR_ASSERT_MESSAGE(second->first, "First node must be known to second node.");
+ VR_ASSERT_MESSAGE(secondData->first, "First node must be known to second node.");
- JointMath& math = second->math();
- Eigen::Vector2f actuators(second->first->getJointValue(), this->getJointValue());
+ hemisphere::CachedMaths& maths = secondData->maths();
+ Eigen::Vector2f actuators(secondData->firstNode->getJointValue(), this->getJointValue());
- math.update(actuators);
+ maths.update(actuators);
- Eigen::Vector3d translation = math.joint.getEndEffectorTranslation();
- const Eigen::Matrix3d rotation = math.joint.getEndEffectorRotation();
+ Eigen::Vector3d translation = maths.maths.getEndEffectorTranslation();
+ const Eigen::Matrix3d rotation = maths.maths.getEndEffectorRotation();
const Eigen::Matrix4d transform = simox::math::pose(translation, rotation);
// Update Second
- {
- this->globalPose = parentPose * localTransformation * transform.cast<float>();
+ this->globalPose = parentPose * localTransformation * transform.cast<float>();
+ const bool verbose = false;
+ if (verbose)
+ {
Eigen::IOFormat iof(5, 0, " ", "\n", " [", "]");
std::cout << __FUNCTION__ << "() of second actuator with "
- << "\n lever = " << math.joint.lever
- << "\n theta0 = " << math.joint.theta0
- << "\n radius = " << math.joint.radius
+ << "\n lever = " << maths.maths.lever
+ << "\n theta0 = " << maths.maths.theta0Rad
+ << "\n radius = " << maths.maths.radius
<< "\n joint value = " << jointValue
<< "\n actuator (angle) = \n" << actuators.transpose().format(iof)
- << "\n actuator (pos) = \n" << math.joint.angleToPosition(actuators.cast<double>()).transpose().format(iof)
+ << "\n actuator (pos) = \n" << maths.maths.angleToPosition(actuators.cast<double>()).transpose().format(iof)
<< "\n local transform = \n" << localTransformation.format(iof)
<< "\n joint transform = \n" << transform.format(iof)
<< std::endl;
@@ -219,7 +207,7 @@ namespace VirtualRobot
void RobotNodeHemisphere::print(bool printChildren, bool printDecoration) const
{
ReadLockPtr lock = getRobot()->getReadLock();
- VR_ASSERT_MESSAGE(first.has_value() xor second.has_value(), std::stringstream() << first.has_value() << " / " << second.has_value());
+ VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(), std::stringstream() << firstData.has_value() << " / " << secondData.has_value());
if (printDecoration)
{
@@ -228,14 +216,14 @@ namespace VirtualRobot
RobotNode::print(false, false);
- if (first)
+ if (firstData)
{
std::cout << "* Hemisphere joint first node";
}
- else if (second)
+ else if (secondData)
{
std::cout << "* Hemisphere joint second node";
- std::cout << "* Transform: \n" << second->math().joint.getEndEffectorTransform() << std::endl;
+ std::cout << "* Transform: \n" << secondData->maths().maths.getEndEffectorTransform() << std::endl;
}
if (printDecoration)
@@ -263,11 +251,11 @@ namespace VirtualRobot
ReadLockPtr lock = getRobot()->getReadLock();
Physics physics = this->physics.scale(scaling);
- RobotNodePtr result;
+ RobotNodeHemispherePtr cloned;
if (optionalDHParameter.isSet)
{
- result.reset(new RobotNodeHemisphere(
- newRobot, name, jointLimitLo, jointLimitHi,
+ cloned.reset(new RobotNodeHemisphere(
+ newRobot, name,
optionalDHParameter.aMM() * scaling,
optionalDHParameter.dMM() * scaling,
optionalDHParameter.alphaRadian(),
@@ -280,15 +268,24 @@ namespace VirtualRobot
{
Eigen::Matrix4f localTransform = getLocalTransformation();
simox::math::position(localTransform) *= scaling;
- result.reset(new RobotNodeHemisphere(
- newRobot, name,
- jointLimitLo, jointLimitHi,
- localTransform, Eigen::Vector3f::Zero(),
+ cloned.reset(new RobotNodeHemisphere(
+ newRobot, name, localTransform,
visualizationModel, collisionModel,
jointValueOffset, physics, colChecker, nodeType));
}
- return result;
+ if (this->firstData)
+ {
+ // We can just copy the math object.
+ cloned->firstData = this->firstData;
+ }
+ else if (this->secondData)
+ {
+ cloned->secondData.emplace(SecondData{});
+ // initialize() takes care of hooking up the second node to the first node.
+ }
+
+ return cloned;
}
@@ -297,6 +294,27 @@ namespace VirtualRobot
return true;
}
+ bool RobotNodeHemisphere::isFirstHemisphereJointNode() const
+ {
+ return firstData.has_value();
+ }
+
+ bool RobotNodeHemisphere::isSecondHemisphereJointNode() const
+ {
+ return secondData.has_value();
+ }
+
+ const RobotNodeHemisphere::SecondData& RobotNodeHemisphere::getSecondData() const
+ {
+ VR_ASSERT(secondData.has_value());
+ return secondData.value();
+ }
+
+ RobotNodeHemisphere::SecondData& RobotNodeHemisphere::getSecondData()
+ {
+ VR_ASSERT(secondData.has_value());
+ return secondData.value();
+ }
void RobotNodeHemisphere::checkValidRobotNodeType()
{
@@ -307,35 +325,61 @@ namespace VirtualRobot
std::string RobotNodeHemisphere::_toXML(const std::string& /*modelPath*/)
{
- VR_ASSERT_MESSAGE(first.has_value() xor second.has_value(), std::stringstream() << first.has_value() << " / " << second.has_value());
+ VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(),
+ std::stringstream() << firstData.has_value() << " / " << secondData.has_value());
- if (first)
+ std::stringstream ss;
+ ss << "\t\t<Joint type='Hemisphere'>" << std::endl;
+ if (firstData)
{
- // TODO
- return "";
+ // Constants are defined in first.
+
+ hemisphere::Maths& maths = firstData->maths.maths;
+ ss << "\t\t\t<hemisphere role='first'"
+ << " lever='" << maths.lever << "'"
+ << " theta0='" << simox::math::rad_to_deg(maths.theta0Rad) << "'"
+ << " />" << std::endl;
}
else
{
- JointMath& math = second->math();
-
- std::stringstream ss;
- ss << "\t\t<Joint type='Hemisphere'>" << std::endl;
- ss << "\t\t\t<hemisphere lever='" << math.joint.lever << "' theta0='" << math.joint.theta0 << "' />" << std::endl;
- ss << "\t\t\t<limits lo='" << jointLimitLo << "' hi='" << jointLimitHi << "' units='radian'/>" << std::endl;
- ss << "\t\t\t<MaxAcceleration value='" << maxAcceleration << "'/>" << std::endl;
- ss << "\t\t\t<MaxVelocity value='" << maxVelocity << "'/>" << std::endl;
- ss << "\t\t\t<MaxTorque value='" << maxTorque << "'/>" << std::endl;
- std::map< std::string, float >::iterator propIt = propagatedJointValues.begin();
-
- while (propIt != propagatedJointValues.end())
- {
- ss << "\t\t\t<PropagateJointValue name='" << propIt->first << "' factor='" << propIt->second << "'/>" << std::endl;
- propIt++;
- }
+ ss << "\t\t\t<hemisphere role='second' />" << std::endl;
+ }
+
+ ss << "\t\t\t<MaxAcceleration value='" << maxAcceleration << "'/>" << std::endl;
+ ss << "\t\t\t<MaxVelocity value='" << maxVelocity << "'/>" << std::endl;
+ ss << "\t\t\t<MaxTorque value='" << maxTorque << "'/>" << std::endl;
- ss << "\t\t</Joint>" << std::endl;
- return ss.str();
+ for (auto propIt = propagatedJointValues.begin(); propIt != propagatedJointValues.end(); ++propIt)
+ {
+ ss << "\t\t\t<PropagateJointValue name='" << propIt->first << "' factor='" << propIt->second << "'/>" << std::endl;
}
+
+ ss << "\t\t</Joint>" << std::endl;
+ return ss.str();
+ }
+
+ hemisphere::CachedMaths& RobotNodeHemisphere::SecondData::maths()
+ {
+ return firstNode->firstData->maths;
+ }
+
+ const hemisphere::CachedMaths& RobotNodeHemisphere::SecondData::maths() const
+ {
+ return firstNode->firstData->maths;
+ }
+
+ hemisphere::Maths::Jacobian RobotNodeHemisphere::SecondData::getJacobian()
+ {
+ VR_ASSERT(firstNode);
+ VR_ASSERT(secondNode);
+
+ hemisphere::CachedMaths& maths = this->maths();
+
+ Eigen::Vector2f actuators(firstNode->getJointValue(), secondNode->getJointValue());
+ maths.update(actuators);
+
+ hemisphere::Maths::Jacobian jacobian = maths.maths.getJacobian();
+ return jacobian;
}
}
diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.h b/VirtualRobot/Nodes/RobotNodeHemisphere.h
index 7a548e60075203990e86d891f6bad4e511e242e8..73c7e8b617fe9fd5be26ae83563fc59df47195ce 100644
--- a/VirtualRobot/Nodes/RobotNodeHemisphere.h
+++ b/VirtualRobot/Nodes/RobotNodeHemisphere.h
@@ -21,10 +21,11 @@
*/
#pragma once
-#include "../VirtualRobot.h"
+#include <VirtualRobot/VirtualRobot.h>
-#include "RobotNode.h"
-#include "HemisphereJoint/Joint.h"
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Nodes/HemisphereJoint/CachedMaths.h>
+#include <VirtualRobot/Nodes/HemisphereJoint/Maths.h>
#include <Eigen/Core>
@@ -38,38 +39,72 @@ namespace VirtualRobot
using RobotNodeHemispherePtr = std::shared_ptr<class RobotNodeHemisphere>;
+ /**
+ * @brief A model of the 2 DoF wrist joint mechanism published in:
+ *
+ * C. Klas and T. Asfour, "A Compact, Lightweight and Singularity-Free
+ * Wrist Joint Mechanism for Humanoid Robots," 2022 IEEE/RSJ International
+ * Conference on Intelligent Robots and Systems (IROS), Kyoto, Japan, 2022,
+ * pp. 457-464, doi: 10.1109/IROS47612.2022.9981787.
+ *
+ * This joint mechanism represents 2 Degrees of Freedom (DoF). Therefore,
+ * it must be represented by two robot nodes in VirtualRobot.
+ */
class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNodeHemisphere : public RobotNode
{
public:
enum class Role
{
+ /// The first DoF in the kinematic chain.
FIRST,
+ /// The second DoF in the kinematic chain.
SECOND,
};
static Role RoleFromString(const std::string& string);
+ /// Information specified in the robot XML.
struct XmlInfo
{
Role role;
// Only set for first:
- double theta0 = -1;
+ double theta0Rad = -1;
double lever = -1;
};
+
+ /// Data held by the first joint.
+ struct FirstData
+ {
+ hemisphere::CachedMaths maths;
+ };
+
+ /// Data held by the second joint.
+ struct SecondData
+ {
+ /// The first actuator node.
+ RobotNodeHemisphere* firstNode = nullptr;
+ RobotNodeHemisphere* secondNode = nullptr;
+
+ hemisphere::CachedMaths& maths();
+ const hemisphere::CachedMaths& maths() const;
+
+ hemisphere::Maths::Jacobian getJacobian();
+ };
+
friend class RobotFactory;
+
+ public:
+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
RobotNodeHemisphere(
RobotWeakPtr rob, ///< The robot
const std::string& name, ///< The name
- 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 (in local joint coord system)
VisualizationNodePtr visualization = nullptr, ///< A visualization model
CollisionModelPtr collisionModel = nullptr, ///< A collision model
float jointValueOffset = 0.0f, ///< An offset that is internally added to the joint value
@@ -78,11 +113,10 @@ namespace VirtualRobot
RobotNodeType type = Generic
);
+ // The DH-based constructor is not tested so far for Hemisphere joints.
RobotNodeHemisphere(
RobotWeakPtr rob, ///< The robot
const std::string& name, ///< The name
- float jointLimitLo, ///< lower joint limit
- float jointLimitHi, ///< upper joint limit
float a, ///< dh paramters
float d, ///< dh paramters
float alpha, ///< dh paramters
@@ -118,6 +152,15 @@ namespace VirtualRobot
bool
isHemisphereJoint() const override;
+ bool isFirstHemisphereJointNode() const;
+ bool isSecondHemisphereJointNode() const;
+
+ /**
+ * @brief Get the data held by the second node.
+ * May only be called if `isFirstHemisphereJointNode()`;
+ */
+ SecondData& getSecondData();
+ const SecondData& getSecondData() const;
protected:
@@ -140,48 +183,18 @@ namespace VirtualRobot
) override;
RobotNodePtr
- _clone(
- const RobotPtr newRobot,
- const VisualizationNodePtr visualizationModel,
- const CollisionModelPtr collisionModel,
- CollisionCheckerPtr colChecker,
- float scaling
- ) override;
+ _clone(const RobotPtr newRobot,
+ const VisualizationNodePtr visualizationModel,
+ const CollisionModelPtr collisionModel,
+ CollisionCheckerPtr colChecker,
+ float scaling
+ ) override;
- protected:
-
- struct JointMath
- {
- /// The actuator values that were used to compute the joint math.
- Eigen::Vector2f actuators = Eigen::Vector2f::Constant(std::numeric_limits<float>::min());
- /// The joint math.
- hemisphere::Joint joint;
-
- void update(const Eigen::Vector2f& actuators);
- };
+ private:
- struct First
- {
- JointMath math;
- };
- std::optional<First> first;
-
- struct Second
- {
- /// The first actuator node.
- RobotNodeHemisphere* first = nullptr;
-
- JointMath& math()
- {
- return first->first->math;
- }
- const JointMath& math() const
- {
- return first->first->math;
- }
- };
- std::optional<Second> second;
+ std::optional<FirstData> firstData;
+ std::optional<SecondData> secondData;
};
diff --git a/VirtualRobot/Nodes/RobotNodeHemisphereFactory.cpp b/VirtualRobot/Nodes/RobotNodeHemisphereFactory.cpp
index 0fade1edc79f45ab2d743d9ed1e9fede66b1946d..52b590d7cab045586daf2a699663d8e4800ba428 100644
--- a/VirtualRobot/Nodes/RobotNodeHemisphereFactory.cpp
+++ b/VirtualRobot/Nodes/RobotNodeHemisphereFactory.cpp
@@ -7,7 +7,8 @@
#include "RobotNodeHemisphereFactory.h"
#include "RobotNode.h"
#include "RobotNodeHemisphere.h"
-#include "../CollisionDetection/CollisionModel.h"
+
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
namespace VirtualRobot
@@ -21,7 +22,8 @@ namespace VirtualRobot
= default;
- RobotNodePtr RobotNodeHemisphereFactory::createRobotNode(
+ RobotNodePtr
+ RobotNodeHemisphereFactory::createRobotNode(
RobotPtr robot,
const std::string& nodeName,
VisualizationNodePtr visualizationModel,
@@ -34,25 +36,26 @@ namespace VirtualRobot
const Eigen::Vector3f& /*translationDirection*/,
const SceneObject::Physics& physics,
RobotNode::RobotNodeType rntype) const
+
{
- std::cout << "CREATE NEW HEMISPHERE JOINT" << std::endl;
+ (void) limitLow, (void) limitHigh;
+ (void) axis;
+
return std::make_shared<RobotNodeHemisphere>(
robot,
nodeName,
- limitLow,
- limitHigh,
preJointTransform,
- axis,
visualizationModel,
collisionModel,
jointValueOffset,
physics,
- (collisionModel ? collisionModel->getCollisionChecker() : CollisionCheckerPtr()),
+ collisionModel ? collisionModel->getCollisionChecker() : CollisionCheckerPtr(),
rntype);
}
- RobotNodePtr RobotNodeHemisphereFactory::createRobotNodeDH(
+ RobotNodePtr
+ RobotNodeHemisphereFactory::createRobotNodeDH(
RobotPtr robot,
const std::string& nodeName,
VisualizationNodePtr visualizationModel,
@@ -64,12 +67,11 @@ namespace VirtualRobot
const SceneObject::Physics& physics,
RobotNode::RobotNodeType rntype) const
{
- std::cout << "CREATE NEW HEMISPHERE JOINT DH" << std::endl;
+ (void) limitLow, (void) limitHigh;
+
return std::make_shared<RobotNodeHemisphere>(
robot,
nodeName,
- limitLow,
- limitHigh,
dhParameters.aMM(),
dhParameters.dMM(),
dhParameters.alphaRadian(),
@@ -78,21 +80,24 @@ namespace VirtualRobot
collisionModel,
jointValueOffset,
physics,
- CollisionCheckerPtr(),
+ collisionModel ? collisionModel->getCollisionChecker() : CollisionCheckerPtr(),
rntype);
}
- RobotNodeFactory::SubClassRegistry RobotNodeHemisphereFactory::registry(RobotNodeHemisphereFactory::getName(), &RobotNodeHemisphereFactory::createInstance);
+ RobotNodeFactory::SubClassRegistry
+ RobotNodeHemisphereFactory::registry(RobotNodeHemisphereFactory::getName(), &RobotNodeHemisphereFactory::createInstance);
- std::string RobotNodeHemisphereFactory::getName()
+ std::string
+ RobotNodeHemisphereFactory::getName()
{
return "hemisphere";
}
- std::shared_ptr<RobotNodeFactory> RobotNodeHemisphereFactory::createInstance(void*)
+ std::shared_ptr<RobotNodeFactory>
+ RobotNodeHemisphereFactory::createInstance(void*)
{
return std::make_shared<RobotNodeHemisphereFactory>();
}
diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp
index 6b5808efc0744a4f3cb575a71c5901a2cfc3957e..b4f1a4c34c0c067645393b9b4a8efc80ccb0cc76 100644
--- a/VirtualRobot/XML/RobotIO.cpp
+++ b/VirtualRobot/XML/RobotIO.cpp
@@ -467,7 +467,7 @@ namespace VirtualRobot
{
case RobotNodeHemisphere::Role::FIRST:
hemisphere->lever = getFloatByAttributeName(node, "lever");
- hemisphere->theta0 = simox::math::deg_to_rad(getFloatByAttributeName(node, "theta0"));
+ hemisphere->theta0Rad = simox::math::deg_to_rad(getFloatByAttributeName(node, "theta0"));
break;
case RobotNodeHemisphere::Role::SECOND:
break;
diff --git a/VirtualRobot/data/robots/examples/HemisphereJoint/Joint3Hemisphere.xml b/VirtualRobot/data/robots/examples/HemisphereJoint/Joint3Hemisphere.xml
index 7cdabb74a2d65856611b5a7f909a84ea42811026..4b4ac38761e6f15152e6b4d7b475658bb66f3e83 100644
--- a/VirtualRobot/data/robots/examples/HemisphereJoint/Joint3Hemisphere.xml
+++ b/VirtualRobot/data/robots/examples/HemisphereJoint/Joint3Hemisphere.xml
@@ -1,26 +1,26 @@
<?xml version="1.0" encoding="UTF-8" ?>
<Robot Type="Simple3DoFRobotWithHemisphereJoint" StandardName="Simple3DoFRobotWithHemisphereJoint" RootNode="First">
-
+
<RobotNode name="First">
<Visualization enable="true">
- <CoordinateAxis enable="true" scaling="1" text="Root"/>
+ <CoordinateAxis enable="true" scaling="1" text="Root"/>
<File type="Inventor">end.iv</File>
- </Visualization>
+ </Visualization>
<Child name="Joint1_Revolute"/>
- </RobotNode>
+ </RobotNode>
<RobotNode name="Joint1_Revolute">
- <Joint type="revolute">
- <Limits unit="degree" lo="-90" hi="90"/>
+ <Joint type="revolute">
+ <Limits unit="degree" lo="-90" hi="90"/>
<Axis x="1" y="0" z="0"/>
</Joint>
- <Visualization enable="true">
+ <Visualization enable="true">
<File type="Inventor">joint_01_base.iv</File>
- <UseAsCollisionModel/>
- </Visualization>
+ <UseAsCollisionModel/>
+ </Visualization>
<Child name="Joint2_Hemisphere_A"/>
- </RobotNode>
+ </RobotNode>
<RobotNode name="Joint2_Hemisphere_A">
<Transform>
@@ -31,10 +31,10 @@
</Joint>
<Visualization enable="true">
<File type="Inventor">joint_02_hemisphere_a.iv</File>
- <UseAsCollisionModel/>
+ <UseAsCollisionModel/>
</Visualization>
<Child name="Joint2_Hemisphere_B"/>
- </RobotNode>
+ </RobotNode>
<RobotNode name="Joint2_Hemisphere_B">
<Joint type="hemisphere">
@@ -48,44 +48,49 @@
</RobotNode>
<RobotNode name="Joint3_Revolute">
- <Transform>
+ <Transform>
<Translation x="0" y="0" z="50" />
- </Transform>
+ </Transform>
<Joint type="revolute">
<Axis x="1" y="0" z="0"/>
- <Limits unit="degree" lo="-90" hi="90"/>
+ <Limits unit="degree" lo="-90" hi="90"/>
</Joint>
- <Visualization enable="true">
+ <Visualization enable="true">
<File type="Inventor">joint_03_finger.iv</File>
- <UseAsCollisionModel/>
- </Visualization>
+ <UseAsCollisionModel/>
+ </Visualization>
<Child name="Last"/>
- </RobotNode>
+ </RobotNode>
<RobotNode name="Last">
- <Transform>
+ <Transform>
<Translation x="0" y="0" z="300" />
- </Transform>
- <Visualization enable="true">
+ </Transform>
+ <Visualization enable="true">
<File type="Inventor">end.iv</File>
- <UseAsCollisionModel/>
- </Visualization>
- </RobotNode>
+ <UseAsCollisionModel/>
+ </Visualization>
+ </RobotNode>
<RobotNodeSet name="AllJoints" kinematicRoot="Joint1_Revolute">
<Node name="Joint1_Revolute"/>
<Node name="Joint2_Hemisphere_A"/>
<Node name="Joint2_Hemisphere_B"/>
<Node name="Joint3_Revolute"/>
- </RobotNodeSet>
+ </RobotNodeSet>
+
+ <RobotNodeSet name="HemisphereJoints" kinematicRoot="Joint2_Hemisphere_A" tcp="Last">
+ <Node name="Joint2_Hemisphere_A"/>
+ <Node name="Joint2_Hemisphere_B"/>
+ </RobotNodeSet>
- <RobotNodeSet name="CollisionModel">
+ <RobotNodeSet name="CollisionModel">
<Node name="First"/>
<Node name="Joint1_Revolute"/>
<Node name="Joint2_Hemisphere_A"/>
<Node name="Joint2_Hemisphere_B"/>
<Node name="Joint3_Revolute"/>
<Node name="Last"/>
- </RobotNodeSet>
+ </RobotNodeSet>
</Robot>
diff --git a/VirtualRobot/examples/HemisphereJoint/main.cpp b/VirtualRobot/examples/HemisphereJoint/main.cpp
index 46693a0c20edf8a282074eb70026189f9b75676d..73f38a97a5c1aa15fbd01bb68a30877d81f14322 100644
--- a/VirtualRobot/examples/HemisphereJoint/main.cpp
+++ b/VirtualRobot/examples/HemisphereJoint/main.cpp
@@ -6,20 +6,17 @@
#include <SimoxUtility/math/statistics/measures.h>
#include <VirtualRobot/RuntimeEnvironment.h>
-#include <VirtualRobot/Nodes/HemisphereJoint/Joint.h>
+#include <VirtualRobot/Nodes/HemisphereJoint/Maths.h>
using VirtualRobot::RuntimeEnvironment;
-/**
- *
- */
int main(int argc, char* argv[])
{
const Eigen::IOFormat iof(5, 0, " ", "\n", " [", "]", "", "");
- RuntimeEnvironment::setCaption("Convert .iv to .wrl files");
+ RuntimeEnvironment::setCaption("Compute hemisphere joint mathematics");
RuntimeEnvironment::considerFlag(
"verbose", "Enable verbose output.");
@@ -67,7 +64,7 @@ int main(int argc, char* argv[])
{
const time_point start = std::chrono::system_clock::now();
- VirtualRobot::hemisphere::Joint joint;
+ VirtualRobot::hemisphere::Maths joint;
joint.computeFkOfPosition(a1, a2);
const Eigen::Vector3d pos = joint.getEndEffectorTranslation();
@@ -109,7 +106,7 @@ int main(int argc, char* argv[])
a1 += offset;
a2 += offset;
- VirtualRobot::hemisphere::Joint joint;
+ VirtualRobot::hemisphere::Maths joint;
joint.computeFkOfPosition(a1, a2);
const Eigen::Vector3d pos = joint.getEndEffectorTranslation();