Skip to content
Snippets Groups Projects
Commit 9d6e19d5 authored by Rainer Kartmann's avatar Rainer Kartmann
Browse files

Merge branch 'hemisphere-joint-allow-setting-joint-limits' into 'master' 

Hemisphere Joint: Allow setting joint limits

See merge request sw/simox/simox!139
parents 6a8b8441 f84e4161
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
VirtualRobot/Nodes/HemisphereJoint/Maths.cpp
+ 38
− 45
View file @ 9d6e19d5
...@@ -5,87 +5,80 @@ ...@@ -5,87 +5,80 @@
#include <SimoxUtility/math/convert/deg_to_rad.h> #include <SimoxUtility/math/convert/deg_to_rad.h>
#include <SimoxUtility/math/pose/pose.h> #include <SimoxUtility/math/pose/pose.h>
namespace VirtualRobot::hemisphere namespace VirtualRobot::hemisphere
{ {
Maths::Maths() : Maths::Maths() : Maths(1, simox::math::deg_to_rad(25.))
Maths(1, simox::math::deg_to_rad(25.))
{ {
} }
Maths::Maths(double lever, double theta0) Maths::Maths(double lever, double theta0)
{ {
this->setConstants(lever, theta0); this->setConstants(lever, theta0, std::nullopt, std::nullopt);
} }
void
void Maths::setConstants(double lever, double theta0) Maths::setConstants(double lever,
double theta0,
std::optional<double> limitLo,
std::optional<double> limitHi)
{ {
this->lever = lever; this->lever = lever;
this->theta0Rad = theta0; this->theta0Rad = theta0;
this->radius = 2 * std::sin(theta0) * lever; this->radiusOfRollingSpheres = std::sin(theta0) * lever;
this->limitHi = simox::math::deg_to_rad(45 - 6.0); this->limitHi = simox::math::deg_to_rad(limitHi.has_value() ? limitHi.value() : (45 - 6.0));
this->limitLo = - simox::math::deg_to_rad(45 - 14.0); this->limitLo =
simox::math::deg_to_rad(limitLo.has_value() ? limitLo.value() : -(45 - 14.0));
} }
void
void Maths::computeFkOfPosition(double p1, double p2) Maths::computeFkOfPosition(double p1, double p2)
{ {
expressions.compute(p1, p2, lever, theta0Rad); expressions.compute(p1, p2, lever, theta0Rad);
} }
void
void Maths::computeFkOfPosition(const ActuatorPosition& p12) Maths::computeFkOfPosition(const ActuatorPosition& p12)
{ {
computeFkOfPosition(p12(0), p12(1)); computeFkOfPosition(p12(0), p12(1));
} }
void
void Maths::computeFkOfAngle(const ActuatorAngle& alpha12) Maths::computeFkOfAngle(const ActuatorAngle& alpha12)
{ {
computeFkOfPosition(angleToPosition(alpha12)); computeFkOfPosition(angleToPosition(alpha12));
} }
Eigen::Vector3d
Eigen::Vector3d Maths::getEndEffectorTranslation() const Maths::getEndEffectorTranslation() const
{ {
return Eigen::Vector3d { return Eigen::Vector3d{expressions.ex, expressions.ey, expressions.ez};
expressions.ex,
expressions.ey,
expressions.ez
};
} }
Eigen::Matrix3d
Eigen::Matrix3d Maths::getEndEffectorRotation() const Maths::getEndEffectorRotation() const
{ {
// r_wrist_to_base = np.array([[exx, eyx, ezx], [exy, eyy, ezy], [exz, eyz, ezz]]) // r_wrist_to_base = np.array([[exx, eyx, ezx], [exy, eyy, ezy], [exz, eyz, ezz]])
Eigen::Matrix3d ori; Eigen::Matrix3d ori;
ori << expressions.exx, expressions.eyx, expressions.ezx, ori << expressions.exx, expressions.eyx, expressions.ezx, expressions.exy, expressions.eyy,
expressions.exy, expressions.eyy, expressions.ezy, expressions.ezy, expressions.exz, expressions.eyz, expressions.ezz;
expressions.exz, expressions.eyz, expressions.ezz;
return ori; return ori;
} }
Eigen::Matrix4d
Eigen::Matrix4d Maths::getEndEffectorTransform() const Maths::getEndEffectorTransform() const
{ {
return simox::math::pose(getEndEffectorTranslation(), getEndEffectorRotation()); return simox::math::pose(getEndEffectorTranslation(), getEndEffectorRotation());
} }
Maths::Jacobian
Maths::Jacobian Maths::getJacobian() const Maths::getJacobian() const
{ {
Maths::Jacobian jacobian; Maths::Jacobian jacobian;
jacobian << expressions.jx1, expressions.jx2, jacobian << expressions.jx1, expressions.jx2, expressions.jy1, expressions.jy2,
expressions.jy1, expressions.jy2, expressions.jz1, expressions.jz2, expressions.jrx1, expressions.jrx2, expressions.jry1,
expressions.jz1, expressions.jz2, expressions.jry2, expressions.jrz1, expressions.jrz2;
expressions.jrx1, expressions.jrx2,
expressions.jry1, expressions.jry2,
expressions.jrz1, expressions.jrz2;
// Current state of constructing the orientational part. // Current state of constructing the orientational part.
// ToDo: Do this with symbolic math inside `Expressions`. // ToDo: Do this with symbolic math inside `Expressions`.
...@@ -95,7 +88,7 @@ namespace VirtualRobot::hemisphere ...@@ -95,7 +88,7 @@ namespace VirtualRobot::hemisphere
for (int column = 0; column < 2; ++column) for (int column = 0; column < 2; ++column)
{ {
// Imagine we apply (+1, 0) / (0, +1) actuator velocities. // Imagine we apply (+1, 0) / (0, +1) actuator velocities.
const Eigen::Vector3d eefVelTrans = jacobian.block<3, 1>(0, column); // * 1.0 const Eigen::Vector3d eefVelTrans = jacobian.block<3, 1>(0, column); // * 1.0
/* /*
* The rotation axis is orthogonal to the vector from origin to the * The rotation axis is orthogonal to the vector from origin to the
...@@ -103,20 +96,20 @@ namespace VirtualRobot::hemisphere ...@@ -103,20 +96,20 @@ namespace VirtualRobot::hemisphere
* *
* For the scaling, ask Cornelius. :) * For the scaling, ask Cornelius. :)
*/ */
const Eigen::Vector3d scaledRotAxis = eefStateTrans.cross(eefVelTrans) const Eigen::Vector3d scaledRotAxis =
/ eefStateTrans.norm() * 2; eefStateTrans.cross(eefVelTrans) / eefStateTrans.norm() * 2;
jacobian.block<3, 1>(3, column) = scaledRotAxis; // / 1.0; jacobian.block<3, 1>(3, column) = scaledRotAxis; // / 1.0;
} }
} }
return jacobian; return jacobian;
} }
Maths::ActuatorPosition
Maths::ActuatorPosition Maths::angleToPosition(const Maths::ActuatorAngle& alpha) const Maths::angleToPosition(const Maths::ActuatorAngle& alpha) const
{ {
return lever * Eigen::sin((alpha + Eigen::Vector2d::Constant(theta0Rad)).array()); return lever * Eigen::sin((alpha + Eigen::Vector2d::Constant(theta0Rad)).array());
} }
} } // namespace VirtualRobot::hemisphere
VirtualRobot/Nodes/HemisphereJoint/Maths.h
+ 8
− 9
View file @ 9d6e19d5
#pragma once #pragma once
#include <optional>
#include <Eigen/Core> #include <Eigen/Core>
#include "Expressions.h" #include "Expressions.h"
namespace VirtualRobot::hemisphere namespace VirtualRobot::hemisphere
{ {
...@@ -18,19 +19,19 @@ namespace VirtualRobot::hemisphere ...@@ -18,19 +19,19 @@ namespace VirtualRobot::hemisphere
class Maths class Maths
{ {
public: public:
using ActuatorPosition = Eigen::Vector2d; using ActuatorPosition = Eigen::Vector2d;
using ActuatorAngle = Eigen::Vector2d; using ActuatorAngle = Eigen::Vector2d;
using Jacobian = Eigen::Matrix<double, 6, 2>; using Jacobian = Eigen::Matrix<double, 6, 2>;
public: public:
Maths(); Maths();
Maths(double lever, double theta0); Maths(double lever, double theta0);
void setConstants(double lever, double theta0); void setConstants(double lever,
double theta0,
std::optional<double> limitLo,
std::optional<double> limitHi);
void computeFkOfAngle(const ActuatorAngle& alpha12); void computeFkOfAngle(const ActuatorAngle& alpha12);
...@@ -48,17 +49,15 @@ namespace VirtualRobot::hemisphere ...@@ -48,17 +49,15 @@ namespace VirtualRobot::hemisphere
public: public:
double lever = 0; double lever = 0;
double theta0Rad = 0; double theta0Rad = 0;
double radius = 0; double radiusOfRollingSpheres = 0;
double limitLo = 0; double limitLo = 0;
double limitHi = 0; double limitHi = 0;
Expressions expressions; Expressions expressions;
}; };
} } // namespace VirtualRobot::hemisphere
VirtualRobot/Nodes/RobotNodeHemisphere.cpp
+ 160
− 114
View file @ 9d6e19d5
#include "RobotNodeHemisphere.h" #include "RobotNodeHemisphere.h"
#include "Nodes/Sensor.h"
#include "Robot.h"
#include "VirtualRobotException.h"
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
#include <Eigen/Geometry> #include <Eigen/Geometry>
#include <SimoxUtility/meta/enum/EnumNames.hpp>
#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/math/convert/rad_to_deg.h> #include <SimoxUtility/math/convert/rad_to_deg.h>
#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/meta/enum/EnumNames.hpp>
#include "Nodes/Sensor.h"
#include "Robot.h"
#include "VirtualRobotException.h"
namespace VirtualRobot namespace VirtualRobot
{ {
static const float initialLimit = 1.0; static const float initialLimit = 1.0;
extern const simox::meta::EnumNames<RobotNodeHemisphere::Role> RoleNames = extern const simox::meta::EnumNames<RobotNodeHemisphere::Role> RoleNames = {
{ {RobotNodeHemisphere::Role::FIRST, "first"},
{ RobotNodeHemisphere::Role::FIRST, "first" }, {RobotNodeHemisphere::Role::SECOND, "second"},
{ RobotNodeHemisphere::Role::SECOND, "second" },
}; };
VirtualRobot::RobotNodeHemisphere::RobotNodeHemisphere() VirtualRobot::RobotNodeHemisphere::RobotNodeHemisphere()
{ {
} }
RobotNodeHemisphere::Role RobotNodeHemisphere::RoleFromString(const std::string& string) RobotNodeHemisphere::Role
RobotNodeHemisphere::RoleFromString(const std::string& string)
{ {
return RoleNames.from_name(string); return RoleNames.from_name(string);
} }
RobotNodeHemisphere::RobotNodeHemisphere( RobotNodeHemisphere::RobotNodeHemisphere(RobotWeakPtr rob,
RobotWeakPtr rob, const std::string& name,
const std::string& name, const Eigen::Matrix4f& preJointTransform,
const Eigen::Matrix4f& preJointTransform, VisualizationNodePtr visualization,
VisualizationNodePtr visualization, CollisionModelPtr collisionModel,
CollisionModelPtr collisionModel, float jointValueOffset,
float jointValueOffset, const SceneObject::Physics& physics,
const SceneObject::Physics& physics, CollisionCheckerPtr colChecker,
CollisionCheckerPtr colChecker, RobotNodeType type) :
RobotNodeType type RobotNode(rob,
) : name,
RobotNode(rob, name, -initialLimit, initialLimit, visualization, collisionModel, -initialLimit,
jointValueOffset, physics, colChecker, type) initialLimit,
visualization,
collisionModel,
jointValueOffset,
physics,
colChecker,
type)
{ {
initialized = false; initialized = false;
optionalDHParameter.isSet = false; optionalDHParameter.isSet = false;
...@@ -53,20 +59,28 @@ namespace VirtualRobot ...@@ -53,20 +59,28 @@ namespace VirtualRobot
checkValidRobotNodeType(); checkValidRobotNodeType();
} }
RobotNodeHemisphere::RobotNodeHemisphere(RobotWeakPtr rob,
RobotNodeHemisphere::RobotNodeHemisphere( const std::string& name,
RobotWeakPtr rob, float a,
const std::string& name, float d,
float a, float d, float alpha, float theta, float alpha,
VisualizationNodePtr visualization, float theta,
CollisionModelPtr collisionModel, VisualizationNodePtr visualization,
float jointValueOffset, CollisionModelPtr collisionModel,
const SceneObject::Physics& physics, float jointValueOffset,
CollisionCheckerPtr colChecker, const SceneObject::Physics& physics,
RobotNodeType type CollisionCheckerPtr colChecker,
) : RobotNodeType type) :
RobotNode(rob, name, -initialLimit, initialLimit, visualization, collisionModel, RobotNode(rob,
jointValueOffset, physics, colChecker, type) name,
-initialLimit,
initialLimit,
visualization,
collisionModel,
jointValueOffset,
physics,
colChecker,
type)
{ {
initialized = false; initialized = false;
optionalDHParameter.isSet = true; optionalDHParameter.isSet = true;
...@@ -95,34 +109,33 @@ namespace VirtualRobot ...@@ -95,34 +109,33 @@ namespace VirtualRobot
checkValidRobotNodeType(); checkValidRobotNodeType();
} }
RobotNodeHemisphere::~RobotNodeHemisphere() = default;
RobotNodeHemisphere::~RobotNodeHemisphere() void
= default; RobotNodeHemisphere::setXmlInfo(const XmlInfo& info)
void RobotNodeHemisphere::setXmlInfo(const XmlInfo& info)
{ {
VR_ASSERT(secondData.has_value()); VR_ASSERT(secondData.has_value());
switch (info.role) switch (info.role)
{ {
case Role::FIRST: case Role::FIRST:
firstData.emplace(FirstData{}); firstData.emplace(FirstData{});
firstData->maths.maths.setConstants(info.lever, info.theta0Rad); firstData->maths.maths.setConstants(
break; info.lever, info.theta0Rad, info.limitLo, info.limitHi);
break;
case Role::SECOND:
secondData.emplace(SecondData{}); case Role::SECOND:
break; secondData.emplace(SecondData{});
break;
} }
} }
bool
bool RobotNodeHemisphere::initialize( RobotNodeHemisphere::initialize(SceneObjectPtr parent,
SceneObjectPtr parent, const std::vector<SceneObjectPtr>& children)
const std::vector<SceneObjectPtr>& children)
{ {
VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(), VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(),
std::stringstream() << firstData.has_value() << " / " << secondData.has_value()); std::stringstream()
<< firstData.has_value() << " / " << secondData.has_value());
// The second node needs to store a reference to the first node. // The second node needs to store a reference to the first node.
if (secondData) if (secondData)
...@@ -132,11 +145,13 @@ namespace VirtualRobot ...@@ -132,11 +145,13 @@ namespace VirtualRobot
RobotNodeHemisphere* firstNode = dynamic_cast<RobotNodeHemisphere*>(parent.get()); RobotNodeHemisphere* firstNode = dynamic_cast<RobotNodeHemisphere*>(parent.get());
RobotNodeHemisphere* secondNode = this; RobotNodeHemisphere* secondNode = this;
if (not (firstNode and firstNode->firstData)) if (not(firstNode and firstNode->firstData))
{ {
std::stringstream ss; std::stringstream ss;
ss << "The parent of a hemisphere joint with role '" << RoleNames.to_name(Role::SECOND) << "' " ss << "The parent of a hemisphere joint with role '"
<< "must be a hemisphere joint with role '" << RoleNames.to_name(Role::FIRST) << "' "; << RoleNames.to_name(Role::SECOND) << "' "
<< "must be a hemisphere joint with role '" << RoleNames.to_name(Role::FIRST)
<< "' ";
THROW_VR_EXCEPTION(ss.str()); THROW_VR_EXCEPTION(ss.str());
} }
...@@ -159,11 +174,12 @@ namespace VirtualRobot ...@@ -159,11 +174,12 @@ namespace VirtualRobot
return RobotNode::initialize(parent, children); return RobotNode::initialize(parent, children);
} }
void
void RobotNodeHemisphere::updateTransformationMatrices( RobotNodeHemisphere::updateTransformationMatrices(const Eigen::Matrix4f& parentPose)
const Eigen::Matrix4f& parentPose)
{ {
VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(), std::stringstream() << firstData.has_value() << " / " << secondData.has_value()); VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(),
std::stringstream()
<< firstData.has_value() << " / " << secondData.has_value());
if (firstData) if (firstData)
{ {
...@@ -174,7 +190,8 @@ namespace VirtualRobot ...@@ -174,7 +190,8 @@ namespace VirtualRobot
VR_ASSERT_MESSAGE(secondData->firstNode, "First node must be known to second node."); VR_ASSERT_MESSAGE(secondData->firstNode, "First node must be known to second node.");
hemisphere::CachedMaths& maths = secondData->maths(); hemisphere::CachedMaths& maths = secondData->maths();
Eigen::Vector2f actuators(secondData->firstNode->getJointValue(), this->getJointValue()); Eigen::Vector2f actuators(secondData->firstNode->getJointValue(),
this->getJointValue());
maths.update(actuators); maths.update(actuators);
...@@ -189,25 +206,34 @@ namespace VirtualRobot ...@@ -189,25 +206,34 @@ namespace VirtualRobot
if (verbose) if (verbose)
{ {
Eigen::IOFormat iof(5, 0, " ", "\n", " [", "]"); Eigen::IOFormat iof(5, 0, " ", "\n", " [", "]");
std::cout << __FUNCTION__ << "() of second actuator with " std::cout
<< "\n lever = " << maths.maths.lever << __FUNCTION__ << "() of second actuator with "
<< "\n theta0 = " << maths.maths.theta0Rad << "\n lever = " << maths.maths.lever
<< "\n radius = " << maths.maths.radius << "\n theta0 = " << maths.maths.theta0Rad
<< "\n joint value = " << jointValue << "\n theta0 = " << maths.maths.theta0Rad << " rad ("
<< "\n actuator (angle) = \n" << actuators.transpose().format(iof) << simox::math::rad_to_deg(maths.maths.theta0Rad) << " deg)"
<< "\n actuator (pos) = \n" << maths.maths.angleToPosition(actuators.cast<double>()).transpose().format(iof) << "\n radius of rolling spheres = " << maths.maths.radiusOfRollingSpheres
<< "\n local transform = \n" << localTransformation.format(iof) << "\n joint limit lo = " << getJointLimitLo() << " rad ("
<< "\n joint transform = \n" << transform.format(iof) << simox::math::rad_to_deg(getJointLimitLo()) << " deg)"
<< std::endl; << "\n joint limit hi = " << getJointLimitHi() << " rad ("
<< simox::math::rad_to_deg(getJointLimitHi()) << " deg)"
<< "\n actuator (angle) = \n"
<< actuators.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;
} }
} }
} }
void
void RobotNodeHemisphere::print(bool printChildren, bool printDecoration) const RobotNodeHemisphere::print(bool printChildren, bool printDecoration) const
{ {
ReadLockPtr lock = getRobot()->getReadLock(); ReadLockPtr lock = getRobot()->getReadLock();
VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(), std::stringstream() << firstData.has_value() << " / " << secondData.has_value()); VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(),
std::stringstream()
<< firstData.has_value() << " / " << secondData.has_value());
if (printDecoration) if (printDecoration)
{ {
...@@ -223,7 +249,8 @@ namespace VirtualRobot ...@@ -223,7 +249,8 @@ namespace VirtualRobot
else if (secondData) else if (secondData)
{ {
std::cout << "* Hemisphere joint second node"; std::cout << "* Hemisphere joint second node";
std::cout << "* Transform: \n" << secondData->maths().maths.getEndEffectorTransform() << std::endl; std::cout << "* Transform: \n"
<< secondData->maths().maths.getEndEffectorTransform() << std::endl;
} }
if (printDecoration) if (printDecoration)
...@@ -240,13 +267,12 @@ namespace VirtualRobot ...@@ -240,13 +267,12 @@ namespace VirtualRobot
} }
} }
RobotNodePtr
RobotNodePtr RobotNodeHemisphere::_clone( RobotNodeHemisphere::_clone(const RobotPtr newRobot,
const RobotPtr newRobot, const VisualizationNodePtr visualizationModel,
const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel,
const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker,
CollisionCheckerPtr colChecker, float scaling)
float scaling)
{ {
ReadLockPtr lock = getRobot()->getReadLock(); ReadLockPtr lock = getRobot()->getReadLock();
Physics physics = this->physics.scale(scaling); Physics physics = this->physics.scale(scaling);
...@@ -254,24 +280,32 @@ namespace VirtualRobot ...@@ -254,24 +280,32 @@ namespace VirtualRobot
RobotNodeHemispherePtr cloned; RobotNodeHemispherePtr cloned;
if (optionalDHParameter.isSet) if (optionalDHParameter.isSet)
{ {
cloned.reset(new RobotNodeHemisphere( cloned.reset(new RobotNodeHemisphere(newRobot,
newRobot, name, name,
optionalDHParameter.aMM() * scaling, optionalDHParameter.aMM() * scaling,
optionalDHParameter.dMM() * scaling, optionalDHParameter.dMM() * scaling,
optionalDHParameter.alphaRadian(), optionalDHParameter.alphaRadian(),
optionalDHParameter.thetaRadian(), optionalDHParameter.thetaRadian(),
visualizationModel, collisionModel, visualizationModel,
jointValueOffset, collisionModel,
physics, colChecker, nodeType)); jointValueOffset,
physics,
colChecker,
nodeType));
} }
else else
{ {
Eigen::Matrix4f localTransform = getLocalTransformation(); Eigen::Matrix4f localTransform = getLocalTransformation();
simox::math::position(localTransform) *= scaling; simox::math::position(localTransform) *= scaling;
cloned.reset(new RobotNodeHemisphere( cloned.reset(new RobotNodeHemisphere(newRobot,
newRobot, name, localTransform, name,
visualizationModel, collisionModel, localTransform,
jointValueOffset, physics, colChecker, nodeType)); visualizationModel,
collisionModel,
jointValueOffset,
physics,
colChecker,
nodeType));
} }
if (this->firstData) if (this->firstData)
...@@ -288,45 +322,52 @@ namespace VirtualRobot ...@@ -288,45 +322,52 @@ namespace VirtualRobot
return cloned; return cloned;
} }
bool
bool RobotNodeHemisphere::isHemisphereJoint() const RobotNodeHemisphere::isHemisphereJoint() const
{ {
return true; return true;
} }
bool RobotNodeHemisphere::isFirstHemisphereJointNode() const bool
RobotNodeHemisphere::isFirstHemisphereJointNode() const
{ {
return firstData.has_value(); return firstData.has_value();
} }
bool RobotNodeHemisphere::isSecondHemisphereJointNode() const bool
RobotNodeHemisphere::isSecondHemisphereJointNode() const
{ {
return secondData.has_value(); return secondData.has_value();
} }
const RobotNodeHemisphere::SecondData& RobotNodeHemisphere::getSecondData() const const RobotNodeHemisphere::SecondData&
RobotNodeHemisphere::getSecondData() const
{ {
VR_ASSERT(secondData.has_value()); VR_ASSERT(secondData.has_value());
return secondData.value(); return secondData.value();
} }
RobotNodeHemisphere::SecondData& RobotNodeHemisphere::getSecondData() RobotNodeHemisphere::SecondData&
RobotNodeHemisphere::getSecondData()
{ {
VR_ASSERT(secondData.has_value()); VR_ASSERT(secondData.has_value());
return secondData.value(); return secondData.value();
} }
void RobotNodeHemisphere::checkValidRobotNodeType() void
RobotNodeHemisphere::checkValidRobotNodeType()
{ {
RobotNode::checkValidRobotNodeType(); RobotNode::checkValidRobotNodeType();
THROW_VR_EXCEPTION_IF(nodeType == Body || nodeType == Transform, "RobotNodeHemisphere must be a JointNode or a GenericNode"); THROW_VR_EXCEPTION_IF(nodeType == Body || nodeType == Transform,
"RobotNodeHemisphere must be a JointNode or a GenericNode");
} }
std::string
std::string RobotNodeHemisphere::_toXML(const std::string& /*modelPath*/) RobotNodeHemisphere::_toXML(const std::string& /*modelPath*/)
{ {
VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(), VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(),
std::stringstream() << firstData.has_value() << " / " << secondData.has_value()); std::stringstream()
<< firstData.has_value() << " / " << secondData.has_value());
std::stringstream ss; std::stringstream ss;
ss << "\t\t<Joint type='Hemisphere'>" << std::endl; ss << "\t\t<Joint type='Hemisphere'>" << std::endl;
...@@ -349,26 +390,31 @@ namespace VirtualRobot ...@@ -349,26 +390,31 @@ namespace VirtualRobot
ss << "\t\t\t<MaxVelocity value='" << maxVelocity << "'/>" << std::endl; ss << "\t\t\t<MaxVelocity value='" << maxVelocity << "'/>" << std::endl;
ss << "\t\t\t<MaxTorque value='" << maxTorque << "'/>" << std::endl; ss << "\t\t\t<MaxTorque value='" << maxTorque << "'/>" << std::endl;
for (auto propIt = propagatedJointValues.begin(); propIt != propagatedJointValues.end(); ++propIt) 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\t<PropagateJointValue name='" << propIt->first << "' factor='"
<< propIt->second << "'/>" << std::endl;
} }
ss << "\t\t</Joint>" << std::endl; ss << "\t\t</Joint>" << std::endl;
return ss.str(); return ss.str();
} }
hemisphere::CachedMaths& RobotNodeHemisphere::SecondData::maths() hemisphere::CachedMaths&
RobotNodeHemisphere::SecondData::maths()
{ {
return firstNode->firstData->maths; return firstNode->firstData->maths;
} }
const hemisphere::CachedMaths& RobotNodeHemisphere::SecondData::maths() const const hemisphere::CachedMaths&
RobotNodeHemisphere::SecondData::maths() const
{ {
return firstNode->firstData->maths; return firstNode->firstData->maths;
} }
hemisphere::Maths::Jacobian RobotNodeHemisphere::SecondData::getJacobian() hemisphere::Maths::Jacobian
RobotNodeHemisphere::SecondData::getJacobian()
{ {
VR_ASSERT(firstNode); VR_ASSERT(firstNode);
VR_ASSERT(secondNode); VR_ASSERT(secondNode);
...@@ -382,4 +428,4 @@ namespace VirtualRobot ...@@ -382,4 +428,4 @@ namespace VirtualRobot
return jacobian; return jacobian;
} }
} } // namespace VirtualRobot
VirtualRobot/Nodes/RobotNodeHemisphere.h
+ 47
− 68
View file @ 9d6e19d5
...@@ -21,18 +21,16 @@ ...@@ -21,18 +21,16 @@
*/ */
#pragma once #pragma once
#include <VirtualRobot/VirtualRobot.h> #include <optional>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Nodes/HemisphereJoint/CachedMaths.h>
#include <VirtualRobot/Nodes/HemisphereJoint/Maths.h>
#include <Eigen/Core>
#include <string> #include <string>
#include <vector> #include <vector>
#include <optional>
#include <Eigen/Core>
#include <VirtualRobot/Nodes/HemisphereJoint/CachedMaths.h>
#include <VirtualRobot/Nodes/HemisphereJoint/Maths.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/VirtualRobot.h>
namespace VirtualRobot namespace VirtualRobot
{ {
...@@ -53,7 +51,6 @@ namespace VirtualRobot ...@@ -53,7 +51,6 @@ namespace VirtualRobot
class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNodeHemisphere : public RobotNode class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNodeHemisphere : public RobotNode
{ {
public: public:
enum class Role enum class Role
{ {
/// The first DoF in the kinematic chain. /// The first DoF in the kinematic chain.
...@@ -71,9 +68,10 @@ namespace VirtualRobot ...@@ -71,9 +68,10 @@ namespace VirtualRobot
// Only set for first: // Only set for first:
double theta0Rad = -1; double theta0Rad = -1;
double lever = -1; double lever = -1;
std::optional<double> limitLo = std::nullopt;
std::optional<double> limitHi = std::nullopt;
}; };
/// Data held by the first joint. /// Data held by the first joint.
struct FirstData struct FirstData
{ {
...@@ -97,60 +95,53 @@ namespace VirtualRobot ...@@ -97,60 +95,53 @@ namespace VirtualRobot
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
RobotNodeHemisphere( RobotNodeHemisphere(
RobotWeakPtr rob, ///< The robot RobotWeakPtr rob, ///< The robot
const std::string& name, ///< The name const std::string& name, ///< The name
const Eigen::Matrix4f& preJointTransform, ///< This transformation is applied before the translation of the joint is done const Eigen::Matrix4f&
VisualizationNodePtr visualization = nullptr, ///< A visualization model preJointTransform, ///< This transformation is applied before the translation of the joint is done
CollisionModelPtr collisionModel = nullptr, ///< A collision model VisualizationNodePtr visualization = nullptr, ///< A visualization model
float jointValueOffset = 0.0f, ///< An offset that is internally added to the joint value CollisionModelPtr collisionModel = nullptr, ///< A collision model
const SceneObject::Physics& p = {}, ///< physics information float jointValueOffset =
CollisionCheckerPtr colChecker = nullptr, ///< A collision checker instance (if not set, the global col checker is used) 0.0f, ///< An offset that is internally added to the joint value
RobotNodeType type = Generic const SceneObject::Physics& p = {}, ///< physics information
); CollisionCheckerPtr colChecker =
nullptr, ///< A collision checker instance (if not set, the global col checker is used)
RobotNodeType type = Generic);
// The DH-based constructor is not tested so far for Hemisphere joints. // The DH-based constructor is not tested so far for Hemisphere joints.
RobotNodeHemisphere( RobotNodeHemisphere(
RobotWeakPtr rob, ///< The robot RobotWeakPtr rob, ///< The robot
const std::string& name, ///< The name const std::string& name, ///< The name
float a, ///< dh paramters float a, ///< dh paramters
float d, ///< dh paramters float d, ///< dh paramters
float alpha, ///< dh paramters float alpha, ///< dh paramters
float theta, ///< dh paramters float theta, ///< dh paramters
VisualizationNodePtr visualization = nullptr, ///< A visualization model VisualizationNodePtr visualization = nullptr, ///< A visualization model
CollisionModelPtr collisionModel = nullptr, ///< A collision model CollisionModelPtr collisionModel = nullptr, ///< A collision model
float jointValueOffset = 0.0f, ///< An offset that is internally added to the joint value float jointValueOffset =
const SceneObject::Physics& p = {}, ///< physics information 0.0f, ///< An offset that is internally added to the joint value
CollisionCheckerPtr colChecker = {}, ///< A collision checker instance (if not set, the global col checker is used) const SceneObject::Physics& p = {}, ///< physics information
RobotNodeType type = Generic CollisionCheckerPtr colChecker =
); {}, ///< A collision checker instance (if not set, the global col checker is used)
RobotNodeType type = Generic);
public: public:
~RobotNodeHemisphere() override; ~RobotNodeHemisphere() override;
void setXmlInfo(const XmlInfo& info); void setXmlInfo(const XmlInfo& info);
bool bool initialize(SceneObjectPtr parent = nullptr,
initialize( const std::vector<SceneObjectPtr>& children = {}) override;
SceneObjectPtr parent = nullptr,
const std::vector<SceneObjectPtr>& children = {}
) override;
/// Print status information. /// Print status information.
void void print(bool printChildren = false, bool printDecoration = true) const override;
print(
bool printChildren = false,
bool printDecoration = true
) const override;
bool bool isHemisphereJoint() const override;
isHemisphereJoint() const override;
bool isFirstHemisphereJointNode() const; bool isFirstHemisphereJointNode() const;
bool isSecondHemisphereJointNode() const; bool isSecondHemisphereJointNode() const;
...@@ -163,39 +154,27 @@ namespace VirtualRobot ...@@ -163,39 +154,27 @@ namespace VirtualRobot
const SecondData& getSecondData() const; const SecondData& getSecondData() const;
protected: protected:
RobotNodeHemisphere(); RobotNodeHemisphere();
/// Derived classes add custom XML tags here /// Derived classes add custom XML tags here
std::string std::string _toXML(const std::string& modelPath) override;
_toXML(
const std::string& modelPath
) override;
/// Checks if nodeType constraints are fulfilled. Otherwise an exception is thrown. /// Checks if nodeType constraints are fulfilled. Otherwise an exception is thrown.
/// Called on initialization. /// Called on initialization.
void void checkValidRobotNodeType() override;
checkValidRobotNodeType() override;
void void updateTransformationMatrices(const Eigen::Matrix4f& parentPose) override;
updateTransformationMatrices(
const Eigen::Matrix4f& parentPose
) override;
RobotNodePtr RobotNodePtr _clone(const RobotPtr newRobot,
_clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel,
const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel,
const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker,
CollisionCheckerPtr colChecker, float scaling) override;
float scaling
) override;
private: private:
std::optional<FirstData> firstData; std::optional<FirstData> firstData;
std::optional<SecondData> secondData; std::optional<SecondData> secondData;
}; };
} // namespace VirtualRobot } // namespace VirtualRobot
VirtualRobot/XML/RobotIO.cpp
+ 435
− 242
View file @ 9d6e19d5
This diff is collapsed.
VirtualRobot/XML/RobotIO.h
+ 74
− 43
View file @ 9d6e19d5
...@@ -22,22 +22,20 @@ ...@@ -22,22 +22,20 @@
*/ */
#pragma once #pragma once
#include "../VirtualRobot.h" #include <map>
#include "../Robot.h"
#include "../Nodes/RobotNode.h"
#include "../EndEffector/EndEffectorActor.h"
#include "BaseIO.h"
#include <string> #include <string>
#include <vector> #include <vector>
#include <map>
#include "../EndEffector/EndEffectorActor.h"
#include "../Nodes/RobotNode.h"
#include "../Robot.h"
#include "../VirtualRobot.h"
#include "BaseIO.h"
// using forward declarations here, so that the rapidXML header does not have to be parsed when this file is included // using forward declarations here, so that the rapidXML header does not have to be parsed when this file is included
namespace rapidxml namespace rapidxml
{ {
template<class Ch> template <class Ch>
class xml_node; class xml_node;
} }
...@@ -47,7 +45,6 @@ namespace VirtualRobot ...@@ -47,7 +45,6 @@ namespace VirtualRobot
class VIRTUAL_ROBOT_IMPORT_EXPORT RobotIO : public BaseIO class VIRTUAL_ROBOT_IMPORT_EXPORT RobotIO : public BaseIO
{ {
public: public:
/*! /*!
Loads robot from file. Loads robot from file.
@param xmlFile The file @param xmlFile The file
...@@ -62,7 +59,9 @@ namespace VirtualRobot ...@@ -62,7 +59,9 @@ namespace VirtualRobot
@param basePath If any \<childFromRobot\> tags are given, the path for searching the robot files can be specified. @param basePath If any \<childFromRobot\> tags are given, the path for searching the robot files can be specified.
@param loadMode Standard: eFull, When eStructure is used no visualization and collision models are loaded for faster access. @param loadMode Standard: eFull, When eStructure is used no visualization and collision models are loaded for faster access.
*/ */
static RobotPtr createRobotFromString(const std::string& xmlString, const std::string& basePath = "", RobotDescription loadMode = eFull); static RobotPtr createRobotFromString(const std::string& xmlString,
const std::string& basePath = "",
RobotDescription loadMode = eFull);
/*! /*!
...@@ -72,7 +71,14 @@ namespace VirtualRobot ...@@ -72,7 +71,14 @@ namespace VirtualRobot
@param basePath The directory to store the robot to @param basePath The directory to store the robot to
@param modelDir The local directory where all visualization files should be stored to. @param modelDir The local directory where all visualization files should be stored to.
*/ */
static bool saveXML(RobotPtr robot, const std::string& filename, const std::string& basePath, const std::string& modelDir = "models", bool storeEEF = true, bool storeRNS = true, bool storeSensors = true, bool storeModelFiles = true); static bool saveXML(RobotPtr robot,
const std::string& filename,
const std::string& basePath,
const std::string& modelDir = "models",
bool storeEEF = true,
bool storeRNS = true,
bool storeSensors = true,
bool storeModelFiles = true);
/*! /*!
...@@ -83,12 +89,12 @@ namespace VirtualRobot ...@@ -83,12 +89,12 @@ namespace VirtualRobot
@param meshDir The local directory where all mesh files should be stored to. @param meshDir The local directory where all mesh files should be stored to.
*/ */
static void saveMJCF(RobotPtr robot, static void saveMJCF(RobotPtr robot,
const std::string& filename, const std::string& basePath, const std::string& filename,
const std::string& basePath,
const std::string& meshDir = "mesh"); const std::string& meshDir = "mesh");
protected: protected:
struct ChildFromRobotDef struct ChildFromRobotDef
{ {
std::string filename; std::string filename;
...@@ -99,42 +105,67 @@ namespace VirtualRobot ...@@ -99,42 +105,67 @@ namespace VirtualRobot
RobotIO(); RobotIO();
~RobotIO() override; ~RobotIO() override;
static RobotPtr processRobot(rapidxml::xml_node<char>* robotXMLNode, const std::string& basePath, RobotDescription loadMode = eFull); static RobotPtr processRobot(rapidxml::xml_node<char>* robotXMLNode,
static RobotPtr processRobotAttributes(rapidxml::xml_node<char>* robotXMLNode, std::string& robotRoot); const std::string& basePath,
static void processRobotChildNodes(rapidxml::xml_node<char>* robotXMLNode, RobotDescription loadMode = eFull);
RobotPtr robo, static RobotPtr processRobotAttributes(rapidxml::xml_node<char>* robotXMLNode,
const std::string& robotRoot, std::string& robotRoot);
const std::string& basePath, static void processRobotChildNodes(
std::map < RobotNodePtr, rapidxml::xml_node<char>* robotXMLNode,
std::vector<ChildFromRobotDef> > & childrenFromRobotFilesMap, RobotPtr robo,
std::vector<rapidxml::xml_node<char>* >& robotNodeSetNodes, const std::string& robotRoot,
std::vector<rapidxml::xml_node<char>* >& endeffectorNodes, const std::string& basePath,
NodeMapping& nodeMapping, std::map<RobotNodePtr, std::vector<ChildFromRobotDef>>& childrenFromRobotFilesMap,
std::optional<HumanMapping>& humanMapping, std::vector<rapidxml::xml_node<char>*>& robotNodeSetNodes,
std::map<std::string, std::vector<std::string>>& attachments, std::vector<rapidxml::xml_node<char>*>& endeffectorNodes,
RobotDescription loadMode = eFull); NodeMapping& nodeMapping,
std::optional<HumanMapping>& humanMapping,
std::map<std::string, std::vector<std::string>>& attachments,
RobotDescription loadMode = eFull);
static RobotNodePtr processRobotNode(rapidxml::xml_node<char>* robotNodeXMLNode, static RobotNodePtr processRobotNode(rapidxml::xml_node<char>* robotNodeXMLNode,
RobotPtr robo, RobotPtr robo,
const std::string& basePath, const std::string& basePath,
int& robotNodeCounter, int& robotNodeCounter,
std::vector< std::string >& childrenNames, std::vector<std::string>& childrenNames,
std::vector< ChildFromRobotDef >& childrenFromRobot, std::vector<ChildFromRobotDef>& childrenFromRobot,
RobotDescription loadMode = eFull, RobotDescription loadMode = eFull,
RobotNode::RobotNodeType rntype = RobotNode::Generic); RobotNode::RobotNodeType rntype = RobotNode::Generic);
static EndEffectorPtr processEndeffectorNode(rapidxml::xml_node<char>* endeffectorXMLNode, RobotPtr robo); static EndEffectorPtr processEndeffectorNode(rapidxml::xml_node<char>* endeffectorXMLNode,
static EndEffectorActorPtr processEndeffectorActorNode(rapidxml::xml_node<char>* endeffectorActorXMLNode, RobotPtr robo); RobotPtr robo);
static void processEndeffectorStaticNode(rapidxml::xml_node<char>* endeffectorStaticXMLNode, RobotPtr robo, std::vector<RobotNodePtr>& staticNodesList); static EndEffectorActorPtr
static EndEffectorActor::CollisionMode processEEFColAttributes(rapidxml::xml_node<char>* node, bool allowOtherAttributes = false); processEndeffectorActorNode(rapidxml::xml_node<char>* endeffectorActorXMLNode,
static void processActorNodeList(rapidxml::xml_node<char>* parentNode, RobotPtr robot, std::vector<EndEffectorActor::ActorDefinition>& actorList, bool clearList = true); RobotPtr robo);
static void processEndeffectorStaticNode(rapidxml::xml_node<char>* endeffectorStaticXMLNode,
RobotPtr robo,
std::vector<RobotNodePtr>& staticNodesList);
static EndEffectorActor::CollisionMode
processEEFColAttributes(rapidxml::xml_node<char>* node, bool allowOtherAttributes = false);
static void processActorNodeList(rapidxml::xml_node<char>* parentNode,
RobotPtr robot,
std::vector<EndEffectorActor::ActorDefinition>& actorList,
bool clearList = true);
//static RobotNodeSetPtr processRobotNodeSet(rapidxml::xml_node<char> *setXMLNode, RobotPtr robo, const std::string &rootName, int &robotNodeSetCounter); //static RobotNodeSetPtr processRobotNodeSet(rapidxml::xml_node<char> *setXMLNode, RobotPtr robo, const std::string &rootName, int &robotNodeSetCounter);
static void processChildNode(rapidxml::xml_node<char>* childXMLNode, std::vector<std::string>& childrenNames); static void processChildNode(rapidxml::xml_node<char>* childXMLNode,
static RobotNodePtr processJointNode(rapidxml::xml_node<char>* jointXMLNode, const std::string& robotNodeName, std::vector<std::string>& childrenNames);
RobotPtr robot, VisualizationNodePtr visualizationNode, CollisionModelPtr collisionModel, static RobotNodePtr processJointNode(rapidxml::xml_node<char>* jointXMLNode,
SceneObject::Physics& physics, RobotNode::RobotNodeType rntype, Eigen::Matrix4f& transformationMatrix); const std::string& robotNodeName,
static void processChildFromRobotNode(rapidxml::xml_node<char>* childXMLNode, const std::string& nodeName, std::vector< ChildFromRobotDef >& childrenFromRobot); RobotPtr robot,
static void processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode, float& jointLimitLo, float& jointLimitHi, bool& limitless); VisualizationNodePtr visualizationNode,
CollisionModelPtr collisionModel,
SceneObject::Physics& physics,
RobotNode::RobotNodeType rntype,
Eigen::Matrix4f& transformationMatrix);
static void processChildFromRobotNode(rapidxml::xml_node<char>* childXMLNode,
const std::string& nodeName,
std::vector<ChildFromRobotDef>& childrenFromRobot);
static void processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode,
float& jointLimitLo,
float& jointLimitHi,
bool& limitless);
static std::map<std::string, int> robot_name_counter; static std::map<std::string, int> robot_name_counter;
static VisualizationNodePtr checkUseAsColModel(rapidxml::xml_node<char>* visuXMLNode, const std::string& robotNodeName, const std::string& basePath); static VisualizationNodePtr checkUseAsColModel(rapidxml::xml_node<char>* visuXMLNode,
const std::string& robotNodeName,
const std::string& basePath);
}; };
} } // namespace VirtualRobot
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment