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 @@
#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() : Maths(1, simox::math::deg_to_rad(25.))
{
}
Maths::Maths(double lever, double theta0)
{
this->setConstants(lever, theta0);
this->setConstants(lever, theta0, std::nullopt, std::nullopt);
}
void Maths::setConstants(double lever, double theta0)
void
Maths::setConstants(double lever,
double theta0,
std::optional<double> limitLo,
std::optional<double> limitHi)
{
this->lever = lever;
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->limitLo = - simox::math::deg_to_rad(45 - 14.0);
this->limitHi = simox::math::deg_to_rad(limitHi.has_value() ? limitHi.value() : (45 - 6.0));
this->limitLo =
simox::math::deg_to_rad(limitLo.has_value() ? limitLo.value() : -(45 - 14.0));
}
void Maths::computeFkOfPosition(double p1, double p2)
void
Maths::computeFkOfPosition(double p1, double p2)
{
expressions.compute(p1, p2, lever, theta0Rad);
}
void Maths::computeFkOfPosition(const ActuatorPosition& p12)
void
Maths::computeFkOfPosition(const ActuatorPosition& p12)
{
computeFkOfPosition(p12(0), p12(1));
}
void Maths::computeFkOfAngle(const ActuatorAngle& alpha12)
void
Maths::computeFkOfAngle(const ActuatorAngle& alpha12)
{
computeFkOfPosition(angleToPosition(alpha12));
}
Eigen::Vector3d Maths::getEndEffectorTranslation() const
Eigen::Vector3d
Maths::getEndEffectorTranslation() const
{
return Eigen::Vector3d {
expressions.ex,
expressions.ey,
expressions.ez
};
return Eigen::Vector3d{expressions.ex, expressions.ey, expressions.ez};
}
Eigen::Matrix3d Maths::getEndEffectorRotation() const
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;
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
Eigen::Matrix4d
Maths::getEndEffectorTransform() const
{
return simox::math::pose(getEndEffectorTranslation(), getEndEffectorRotation());
}
Maths::Jacobian Maths::getJacobian() const
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;
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`.
......@@ -95,7 +88,7 @@ namespace VirtualRobot::hemisphere
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
const Eigen::Vector3d eefVelTrans = jacobian.block<3, 1>(0, column); // * 1.0
/*
* The rotation axis is orthogonal to the vector from origin to the
......@@ -103,20 +96,20 @@ namespace VirtualRobot::hemisphere
*
* For the scaling, ask Cornelius. :)
*/
const Eigen::Vector3d scaledRotAxis = eefStateTrans.cross(eefVelTrans)
/ eefStateTrans.norm() * 2;
const Eigen::Vector3d scaledRotAxis =
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;
}
Maths::ActuatorPosition Maths::angleToPosition(const Maths::ActuatorAngle& alpha) const
Maths::ActuatorPosition
Maths::angleToPosition(const Maths::ActuatorAngle& alpha) const
{
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
#include <optional>
#include <Eigen/Core>
#include "Expressions.h"
namespace VirtualRobot::hemisphere
{
......@@ -18,19 +19,19 @@ namespace VirtualRobot::hemisphere
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 setConstants(double lever,
double theta0,
std::optional<double> limitLo,
std::optional<double> limitHi);
void computeFkOfAngle(const ActuatorAngle& alpha12);
......@@ -48,17 +49,15 @@ namespace VirtualRobot::hemisphere
public:
double lever = 0;
double theta0Rad = 0;
double radius = 0;
double radiusOfRollingSpheres = 0;
double limitLo = 0;
double limitHi = 0;
Expressions expressions;
};
}
} // namespace VirtualRobot::hemisphere
VirtualRobot/Nodes/RobotNodeHemisphere.cpp
+ 160
− 114
View file @ 9d6e19d5
#include "RobotNodeHemisphere.h"
#include "Nodes/Sensor.h"
#include "Robot.h"
#include "VirtualRobotException.h"
#include <algorithm>
#include <cmath>
#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/pose/pose.h>
#include <SimoxUtility/meta/enum/EnumNames.hpp>
#include "Nodes/Sensor.h"
#include "Robot.h"
#include "VirtualRobotException.h"
namespace VirtualRobot
{
static const float initialLimit = 1.0;
extern const simox::meta::EnumNames<RobotNodeHemisphere::Role> RoleNames =
{
{ RobotNodeHemisphere::Role::FIRST, "first" },
{ RobotNodeHemisphere::Role::SECOND, "second" },
extern const simox::meta::EnumNames<RobotNodeHemisphere::Role> RoleNames = {
{RobotNodeHemisphere::Role::FIRST, "first"},
{RobotNodeHemisphere::Role::SECOND, "second"},
};
VirtualRobot::RobotNodeHemisphere::RobotNodeHemisphere()
{
}
RobotNodeHemisphere::Role RobotNodeHemisphere::RoleFromString(const std::string& string)
RobotNodeHemisphere::Role
RobotNodeHemisphere::RoleFromString(const std::string& string)
{
return RoleNames.from_name(string);
}
RobotNodeHemisphere::RobotNodeHemisphere(
RobotWeakPtr rob,
const std::string& name,
const Eigen::Matrix4f& preJointTransform,
VisualizationNodePtr visualization,
CollisionModelPtr collisionModel,
float jointValueOffset,
const SceneObject::Physics& physics,
CollisionCheckerPtr colChecker,
RobotNodeType type
) :
RobotNode(rob, name, -initialLimit, initialLimit, visualization, collisionModel,
jointValueOffset, physics, colChecker, type)
RobotNodeHemisphere::RobotNodeHemisphere(RobotWeakPtr rob,
const std::string& name,
const Eigen::Matrix4f& preJointTransform,
VisualizationNodePtr visualization,
CollisionModelPtr collisionModel,
float jointValueOffset,
const SceneObject::Physics& physics,
CollisionCheckerPtr colChecker,
RobotNodeType type) :
RobotNode(rob,
name,
-initialLimit,
initialLimit,
visualization,
collisionModel,
jointValueOffset,
physics,
colChecker,
type)
{
initialized = false;
optionalDHParameter.isSet = false;
......@@ -53,20 +59,28 @@ namespace VirtualRobot
checkValidRobotNodeType();
}
RobotNodeHemisphere::RobotNodeHemisphere(
RobotWeakPtr rob,
const std::string& name,
float a, float d, float alpha, float theta,
VisualizationNodePtr visualization,
CollisionModelPtr collisionModel,
float jointValueOffset,
const SceneObject::Physics& physics,
CollisionCheckerPtr colChecker,
RobotNodeType type
) :
RobotNode(rob, name, -initialLimit, initialLimit, visualization, collisionModel,
jointValueOffset, physics, colChecker, type)
RobotNodeHemisphere::RobotNodeHemisphere(RobotWeakPtr rob,
const std::string& name,
float a,
float d,
float alpha,
float theta,
VisualizationNodePtr visualization,
CollisionModelPtr collisionModel,
float jointValueOffset,
const SceneObject::Physics& physics,
CollisionCheckerPtr colChecker,
RobotNodeType type) :
RobotNode(rob,
name,
-initialLimit,
initialLimit,
visualization,
collisionModel,
jointValueOffset,
physics,
colChecker,
type)
{
initialized = false;
optionalDHParameter.isSet = true;
......@@ -95,34 +109,33 @@ namespace VirtualRobot
checkValidRobotNodeType();
}
RobotNodeHemisphere::~RobotNodeHemisphere() = default;
RobotNodeHemisphere::~RobotNodeHemisphere()
= default;
void RobotNodeHemisphere::setXmlInfo(const XmlInfo& info)
void
RobotNodeHemisphere::setXmlInfo(const XmlInfo& info)
{
VR_ASSERT(secondData.has_value());
switch (info.role)
{
case Role::FIRST:
firstData.emplace(FirstData{});
firstData->maths.maths.setConstants(info.lever, info.theta0Rad);
break;
case Role::SECOND:
secondData.emplace(SecondData{});
break;
case Role::FIRST:
firstData.emplace(FirstData{});
firstData->maths.maths.setConstants(
info.lever, info.theta0Rad, info.limitLo, info.limitHi);
break;
case Role::SECOND:
secondData.emplace(SecondData{});
break;
}
}
bool RobotNodeHemisphere::initialize(
SceneObjectPtr parent,
const std::vector<SceneObjectPtr>& children)
bool
RobotNodeHemisphere::initialize(SceneObjectPtr parent,
const std::vector<SceneObjectPtr>& children)
{
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.
if (secondData)
......@@ -132,11 +145,13 @@ namespace VirtualRobot
RobotNodeHemisphere* firstNode = dynamic_cast<RobotNodeHemisphere*>(parent.get());
RobotNodeHemisphere* secondNode = this;
if (not (firstNode and firstNode->firstData))
if (not(firstNode and firstNode->firstData))
{
std::stringstream ss;
ss << "The parent of a hemisphere joint with role '" << RoleNames.to_name(Role::SECOND) << "' "
<< "must be a hemisphere joint with role '" << RoleNames.to_name(Role::FIRST) << "' ";
ss << "The parent of a hemisphere joint with role '"
<< RoleNames.to_name(Role::SECOND) << "' "
<< "must be a hemisphere joint with role '" << RoleNames.to_name(Role::FIRST)
<< "' ";
THROW_VR_EXCEPTION(ss.str());
}
......@@ -159,11 +174,12 @@ namespace VirtualRobot
return RobotNode::initialize(parent, children);
}
void RobotNodeHemisphere::updateTransformationMatrices(
const Eigen::Matrix4f& parentPose)
void
RobotNodeHemisphere::updateTransformationMatrices(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)
{
......@@ -174,7 +190,8 @@ namespace VirtualRobot
VR_ASSERT_MESSAGE(secondData->firstNode, "First node must be known to second node.");
hemisphere::CachedMaths& maths = secondData->maths();
Eigen::Vector2f actuators(secondData->firstNode->getJointValue(), this->getJointValue());
Eigen::Vector2f actuators(secondData->firstNode->getJointValue(),
this->getJointValue());
maths.update(actuators);
......@@ -189,25 +206,34 @@ namespace VirtualRobot
if (verbose)
{
Eigen::IOFormat iof(5, 0, " ", "\n", " [", "]");
std::cout << __FUNCTION__ << "() of second actuator with "
<< "\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" << 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;
std::cout
<< __FUNCTION__ << "() of second actuator with "
<< "\n lever = " << maths.maths.lever
<< "\n theta0 = " << maths.maths.theta0Rad
<< "\n theta0 = " << maths.maths.theta0Rad << " rad ("
<< simox::math::rad_to_deg(maths.maths.theta0Rad) << " deg)"
<< "\n radius of rolling spheres = " << maths.maths.radiusOfRollingSpheres
<< "\n joint limit lo = " << getJointLimitLo() << " rad ("
<< simox::math::rad_to_deg(getJointLimitLo()) << " deg)"
<< "\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 RobotNodeHemisphere::print(bool printChildren, bool printDecoration) const
void
RobotNodeHemisphere::print(bool printChildren, bool printDecoration) const
{
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)
{
......@@ -223,7 +249,8 @@ namespace VirtualRobot
else if (secondData)
{
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)
......@@ -240,13 +267,12 @@ namespace VirtualRobot
}
}
RobotNodePtr RobotNodeHemisphere::_clone(
const RobotPtr newRobot,
const VisualizationNodePtr visualizationModel,
const CollisionModelPtr collisionModel,
CollisionCheckerPtr colChecker,
float scaling)
RobotNodePtr
RobotNodeHemisphere::_clone(const RobotPtr newRobot,
const VisualizationNodePtr visualizationModel,
const CollisionModelPtr collisionModel,
CollisionCheckerPtr colChecker,
float scaling)
{
ReadLockPtr lock = getRobot()->getReadLock();
Physics physics = this->physics.scale(scaling);
......@@ -254,24 +280,32 @@ namespace VirtualRobot
RobotNodeHemispherePtr cloned;
if (optionalDHParameter.isSet)
{
cloned.reset(new RobotNodeHemisphere(
newRobot, name,
optionalDHParameter.aMM() * scaling,
optionalDHParameter.dMM() * scaling,
optionalDHParameter.alphaRadian(),
optionalDHParameter.thetaRadian(),
visualizationModel, collisionModel,
jointValueOffset,
physics, colChecker, nodeType));
cloned.reset(new RobotNodeHemisphere(newRobot,
name,
optionalDHParameter.aMM() * scaling,
optionalDHParameter.dMM() * scaling,
optionalDHParameter.alphaRadian(),
optionalDHParameter.thetaRadian(),
visualizationModel,
collisionModel,
jointValueOffset,
physics,
colChecker,
nodeType));
}
else
{
Eigen::Matrix4f localTransform = getLocalTransformation();
simox::math::position(localTransform) *= scaling;
cloned.reset(new RobotNodeHemisphere(
newRobot, name, localTransform,
visualizationModel, collisionModel,
jointValueOffset, physics, colChecker, nodeType));
cloned.reset(new RobotNodeHemisphere(newRobot,
name,
localTransform,
visualizationModel,
collisionModel,
jointValueOffset,
physics,
colChecker,
nodeType));
}
if (this->firstData)
......@@ -288,45 +322,52 @@ namespace VirtualRobot
return cloned;
}
bool RobotNodeHemisphere::isHemisphereJoint() const
bool
RobotNodeHemisphere::isHemisphereJoint() const
{
return true;
}
bool RobotNodeHemisphere::isFirstHemisphereJointNode() const
bool
RobotNodeHemisphere::isFirstHemisphereJointNode() const
{
return firstData.has_value();
}
bool RobotNodeHemisphere::isSecondHemisphereJointNode() const
bool
RobotNodeHemisphere::isSecondHemisphereJointNode() const
{
return secondData.has_value();
}
const RobotNodeHemisphere::SecondData& RobotNodeHemisphere::getSecondData() const
const RobotNodeHemisphere::SecondData&
RobotNodeHemisphere::getSecondData() const
{
VR_ASSERT(secondData.has_value());
return secondData.value();
}
RobotNodeHemisphere::SecondData& RobotNodeHemisphere::getSecondData()
RobotNodeHemisphere::SecondData&
RobotNodeHemisphere::getSecondData()
{
VR_ASSERT(secondData.has_value());
return secondData.value();
}
void RobotNodeHemisphere::checkValidRobotNodeType()
void
RobotNodeHemisphere::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 RobotNodeHemisphere::_toXML(const std::string& /*modelPath*/)
std::string
RobotNodeHemisphere::_toXML(const std::string& /*modelPath*/)
{
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;
ss << "\t\t<Joint type='Hemisphere'>" << std::endl;
......@@ -349,26 +390,31 @@ namespace VirtualRobot
ss << "\t\t\t<MaxVelocity value='" << maxVelocity << "'/>" << 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;
return ss.str();
}
hemisphere::CachedMaths& RobotNodeHemisphere::SecondData::maths()
hemisphere::CachedMaths&
RobotNodeHemisphere::SecondData::maths()
{
return firstNode->firstData->maths;
}
const hemisphere::CachedMaths& RobotNodeHemisphere::SecondData::maths() const
const hemisphere::CachedMaths&
RobotNodeHemisphere::SecondData::maths() const
{
return firstNode->firstData->maths;
}
hemisphere::Maths::Jacobian RobotNodeHemisphere::SecondData::getJacobian()
hemisphere::Maths::Jacobian
RobotNodeHemisphere::SecondData::getJacobian()
{
VR_ASSERT(firstNode);
VR_ASSERT(secondNode);
......@@ -382,4 +428,4 @@ namespace VirtualRobot
return jacobian;
}
}
} // namespace VirtualRobot
VirtualRobot/Nodes/RobotNodeHemisphere.h
+ 47
− 68
View file @ 9d6e19d5
......@@ -21,18 +21,16 @@
*/
#pragma once
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Nodes/HemisphereJoint/CachedMaths.h>
#include <VirtualRobot/Nodes/HemisphereJoint/Maths.h>
#include <Eigen/Core>
#include <optional>
#include <string>
#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
{
......@@ -53,7 +51,6 @@ namespace VirtualRobot
class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNodeHemisphere : public RobotNode
{
public:
enum class Role
{
/// The first DoF in the kinematic chain.
......@@ -71,9 +68,10 @@ namespace VirtualRobot
// Only set for first:
double theta0Rad = -1;
double lever = -1;
std::optional<double> limitLo = std::nullopt;
std::optional<double> limitHi = std::nullopt;
};
/// Data held by the first joint.
struct FirstData
{
......@@ -97,60 +95,53 @@ namespace VirtualRobot
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
RobotNodeHemisphere(
RobotWeakPtr rob, ///< The robot
const std::string& name, ///< The name
const Eigen::Matrix4f& preJointTransform, ///< This transformation is applied before the translation of the joint is done
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
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
);
RobotWeakPtr rob, ///< The robot
const std::string& name, ///< The name
const Eigen::Matrix4f&
preJointTransform, ///< This transformation is applied before the translation of the joint is done
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
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.
RobotNodeHemisphere(
RobotWeakPtr rob, ///< The robot
const std::string& name, ///< The name
float a, ///< dh paramters
float d, ///< dh paramters
float alpha, ///< dh paramters
float theta, ///< dh paramters
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
const SceneObject::Physics& p = {}, ///< physics information
CollisionCheckerPtr colChecker = {}, ///< A collision checker instance (if not set, the global col checker is used)
RobotNodeType type = Generic
);
RobotWeakPtr rob, ///< The robot
const std::string& name, ///< The name
float a, ///< dh paramters
float d, ///< dh paramters
float alpha, ///< dh paramters
float theta, ///< dh paramters
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
const SceneObject::Physics& p = {}, ///< physics information
CollisionCheckerPtr colChecker =
{}, ///< A collision checker instance (if not set, the global col checker is used)
RobotNodeType type = Generic);
public:
~RobotNodeHemisphere() override;
void setXmlInfo(const XmlInfo& info);
bool
initialize(
SceneObjectPtr parent = nullptr,
const std::vector<SceneObjectPtr>& children = {}
) override;
bool initialize(SceneObjectPtr parent = nullptr,
const std::vector<SceneObjectPtr>& children = {}) override;
/// Print status information.
void
print(
bool printChildren = false,
bool printDecoration = true
) const override;
void print(bool printChildren = false, bool printDecoration = true) const override;
bool
isHemisphereJoint() const override;
bool isHemisphereJoint() const override;
bool isFirstHemisphereJointNode() const;
bool isSecondHemisphereJointNode() const;
......@@ -163,39 +154,27 @@ namespace VirtualRobot
const SecondData& getSecondData() const;
protected:
RobotNodeHemisphere();
/// Derived classes add custom XML tags here
std::string
_toXML(
const std::string& modelPath
) override;
std::string _toXML(const std::string& modelPath) override;
/// Checks if nodeType constraints are fulfilled. Otherwise an exception is thrown.
/// Called on initialization.
void
checkValidRobotNodeType() override;
void checkValidRobotNodeType() override;
void
updateTransformationMatrices(
const Eigen::Matrix4f& parentPose
) override;
void updateTransformationMatrices(const Eigen::Matrix4f& parentPose) override;
RobotNodePtr
_clone(const RobotPtr newRobot,
const VisualizationNodePtr visualizationModel,
const CollisionModelPtr collisionModel,
CollisionCheckerPtr colChecker,
float scaling
) override;
RobotNodePtr _clone(const RobotPtr newRobot,
const VisualizationNodePtr visualizationModel,
const CollisionModelPtr collisionModel,
CollisionCheckerPtr colChecker,
float scaling) override;
private:
std::optional<FirstData> firstData;
std::optional<SecondData> secondData;
};
} // 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 @@
*/
#pragma once
#include "../VirtualRobot.h"
#include "../Robot.h"
#include "../Nodes/RobotNode.h"
#include "../EndEffector/EndEffectorActor.h"
#include "BaseIO.h"
#include <map>
#include <string>
#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
namespace rapidxml
{
template<class Ch>
template <class Ch>
class xml_node;
}
......@@ -47,7 +45,6 @@ namespace VirtualRobot
class VIRTUAL_ROBOT_IMPORT_EXPORT RobotIO : public BaseIO
{
public:
/*!
Loads robot from file.
@param xmlFile The file
......@@ -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 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
@param basePath The directory to store the robot 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
@param meshDir The local directory where all mesh files should be stored to.
*/
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");
protected:
struct ChildFromRobotDef
{
std::string filename;
......@@ -99,42 +105,67 @@ namespace VirtualRobot
RobotIO();
~RobotIO() override;
static RobotPtr processRobot(rapidxml::xml_node<char>* robotXMLNode, const std::string& basePath, RobotDescription loadMode = eFull);
static RobotPtr processRobotAttributes(rapidxml::xml_node<char>* robotXMLNode, std::string& robotRoot);
static void processRobotChildNodes(rapidxml::xml_node<char>* robotXMLNode,
RobotPtr robo,
const std::string& robotRoot,
const std::string& basePath,
std::map < RobotNodePtr,
std::vector<ChildFromRobotDef> > & childrenFromRobotFilesMap,
std::vector<rapidxml::xml_node<char>* >& robotNodeSetNodes,
std::vector<rapidxml::xml_node<char>* >& endeffectorNodes,
NodeMapping& nodeMapping,
std::optional<HumanMapping>& humanMapping,
std::map<std::string, std::vector<std::string>>& attachments,
RobotDescription loadMode = eFull);
static RobotPtr processRobot(rapidxml::xml_node<char>* robotXMLNode,
const std::string& basePath,
RobotDescription loadMode = eFull);
static RobotPtr processRobotAttributes(rapidxml::xml_node<char>* robotXMLNode,
std::string& robotRoot);
static void processRobotChildNodes(
rapidxml::xml_node<char>* robotXMLNode,
RobotPtr robo,
const std::string& robotRoot,
const std::string& basePath,
std::map<RobotNodePtr, std::vector<ChildFromRobotDef>>& childrenFromRobotFilesMap,
std::vector<rapidxml::xml_node<char>*>& robotNodeSetNodes,
std::vector<rapidxml::xml_node<char>*>& endeffectorNodes,
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,
RobotPtr robo,
const std::string& basePath,
int& robotNodeCounter,
std::vector< std::string >& childrenNames,
std::vector< ChildFromRobotDef >& childrenFromRobot,
std::vector<std::string>& childrenNames,
std::vector<ChildFromRobotDef>& childrenFromRobot,
RobotDescription loadMode = eFull,
RobotNode::RobotNodeType rntype = RobotNode::Generic);
static EndEffectorPtr processEndeffectorNode(rapidxml::xml_node<char>* endeffectorXMLNode, RobotPtr robo);
static EndEffectorActorPtr processEndeffectorActorNode(rapidxml::xml_node<char>* endeffectorActorXMLNode, 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 EndEffectorPtr processEndeffectorNode(rapidxml::xml_node<char>* endeffectorXMLNode,
RobotPtr robo);
static EndEffectorActorPtr
processEndeffectorActorNode(rapidxml::xml_node<char>* endeffectorActorXMLNode,
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 void processChildNode(rapidxml::xml_node<char>* childXMLNode, std::vector<std::string>& childrenNames);
static RobotNodePtr processJointNode(rapidxml::xml_node<char>* jointXMLNode, const std::string& robotNodeName,
RobotPtr robot, 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 void processChildNode(rapidxml::xml_node<char>* childXMLNode,
std::vector<std::string>& childrenNames);
static RobotNodePtr processJointNode(rapidxml::xml_node<char>* jointXMLNode,
const std::string& robotNodeName,
RobotPtr robot,
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 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