diff --git a/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp
index f35569f728c3412be25276ca6d6fce59da010786..de9c5116758c403da1fb0d23b3b750d5837bce7f 100644
--- a/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp
+++ b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp
@@ -91,7 +91,27 @@ CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params, SolveState &s)
s.jointRegularization = Eigen::VectorXf::Zero(s.cols);
for (size_t i = 0; i < rns->getSize(); i++)
{
- s.jointRegularization(i) = rns->getNode(i)->isTranslationalJoint() ? params.jointRegularizationTranslation : params.jointRegularizationRotation;
+ float regularization = 1;
+ if (rns->getNode(i)->isTranslationalJoint())
+ {
+ regularization = params.jointRegularizationTranslation;
+ }
+ else if (rns->getNode(i)->isRotationalJoint())
+ {
+ regularization = params.jointRegularizationRotation;
+ }
+ else if (rns->getNode(i)->isHemisphereJoint())
+ {
+ // FIXME: ToDo?
+ regularization = params.jointRegularizationRotation;
+ }
+ else if (rns->getNode(i)->isFourBarJoint())
+ {
+ // FIXME: ToDo?
+ regularization = params.jointRegularizationRotation;
+ }
+
+ s.jointRegularization(i) = regularization;
}
s.cartesianRegularization = Eigen::VectorXf::Zero(s.rows);
@@ -206,39 +226,48 @@ 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;
+ std::cout << "Before: " << jv << std::endl;
+
jv = jv * params.stepSize;
jv = LimitInfNormTo(jv, params.maxJointAngleStep, params.maxJointAngleStepIgnore);
jv = jv.cwiseProduct(s.jointRegularization);
+ std::cout << "After: " << jv << std::endl;
+
Eigen::VectorXf newJointValues = s.jointValues + jv;
rns->setJointValues(newJointValues);
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 94da3ade3a5c5e6f151365bb0427c60d21262a58..0e2471ec3d0c768d64bc90cbaa299e13a4ed7a3b 100644
--- a/VirtualRobot/IK/DifferentialIK.cpp
+++ b/VirtualRobot/IK/DifferentialIK.cpp
@@ -6,10 +6,9 @@
#include "../Nodes/RobotNodeRevolute.h"
#include "../VirtualRobotException.h"
#include "../CollisionDetection/CollisionChecker.h"
-#include "Nodes/FourBar/Joint.h"
-#include "Nodes/RobotNodeFourBar.h"
-#include "Nodes/RobotNodeHemisphere.h"
-#include "VirtualRobot.h"
+#include <VirtualRobot/Nodes/RobotNodeHemisphere.h>
+#include <VirtualRobot/Nodes/RobotNodeFourBar.h>
+#include <VirtualRobot/Nodes/RobotNodeHemisphere.h>
#include <Eigen/Geometry>
#include <Eigen/src/Core/Matrix.h>
@@ -385,8 +384,11 @@ 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)
{
+ bool isHemisphere = dof->isHemisphereJoint();
+ std::cout << "isHemisphere: " << isHemisphere << std::endl;
// Calculus for rotational joints is different as for prismatic joints.
if (dof->isRotationalJoint())
@@ -450,8 +452,63 @@ namespace VirtualRobot
}
else if (dof->isHemisphereJoint())
{
- const RobotNodeHemisphere* hemisphere = dynamic_cast<RobotNodeHemisphere*>(dof.get());
- // FIXME implement
+ RobotNodeHemispherePtr hemisphere
+ = std::dynamic_pointer_cast<RobotNodeHemisphere>(dof);
+
+ if (not (hemisphere->first.has_value() xor hemisphere->second.has_value()))
+ {
+ throw "!!!!";
+ }
+
+ if (hemisphere->first)
+ {
+ std::cout << "First Hemisphere" << std::endl;
+ }
+ else if (hemisphere->second)
+ {
+ std::cout << "Second Hemisphere" << std::endl;
+
+ RobotNodeHemispherePtr second = hemisphere;
+ RobotNodeHemisphere* first = hemisphere->second->first;
+
+ RobotNodeHemisphere::JointMath& math = first->first->math;
+
+ Eigen::Vector2f actuators(first->getJointValue(), second->getJointValue());
+ math.update(actuators);
+
+ hemisphere::Joint::Jacobian jacobian = math.joint.getJacobian();
+
+ tmpUpdateJacobianPosition.block<3, 2>(0, i-1) =
+ jacobian.block<3, 2>(0, 0).cast<float>();
+
+ {
+ // Assume we move with (+1, +1)
+ Eigen::Vector3d eefStateTrans = math.joint.getEndEffectorTranslation();
+
+ Eigen::Vector2d actuatorVel = Eigen::Vector2d::Ones();
+ Eigen::Vector3d eefVelTrans = jacobian.block<3, 2>(0, 0) * actuatorVel;
+
+ Eigen::Vector3d rotAxis = eefStateTrans.cross(eefVelTrans) / eefStateTrans.norm() * 2;
+
+ // Left column.
+ for (int column = 0; column < 2; ++column)
+ {
+ if (actuatorVel(column) != 0)
+ {
+ tmpUpdateJacobianOrientation.block<3, 1>(0, (i-1) + column) =
+ (rotAxis / actuatorVel(column)).cast<float>();
+ }
+ else
+ {
+ tmpUpdateJacobianOrientation.block<3, 1>(0, (i-1) + column).setZero();
+ }
+ }
+ }
+ }
+ else
+ {
+ // Pass
+ }
}
else if (dof->isFourBarJoint())
{
@@ -512,7 +569,6 @@ namespace VirtualRobot
}
-
}
#ifdef CHECK_PERFORMANCE
diff --git a/VirtualRobot/Nodes/HemisphereJoint/Joint.cpp b/VirtualRobot/Nodes/HemisphereJoint/Joint.cpp
index 1d5a26ad82b6099338568961f1008b3cd415cf9d..b795fc833fd41d27f7c64996e08e700752182393 100644
--- a/VirtualRobot/Nodes/HemisphereJoint/Joint.cpp
+++ b/VirtualRobot/Nodes/HemisphereJoint/Joint.cpp
@@ -38,13 +38,13 @@ namespace VirtualRobot::hemisphere
}
- void Joint::computeFkOfPosition(const Eigen::Vector2d& p12)
+ void Joint::computeFkOfPosition(const ActuatorPosition& p12)
{
computeFkOfPosition(p12(0), p12(1));
}
- void Joint::computeFkOfAngle(const Eigen::Vector2d& alpha12)
+ void Joint::computeFkOfAngle(const ActuatorAngle& alpha12)
{
computeFkOfPosition(angleToPosition(alpha12));
}
@@ -89,7 +89,8 @@ namespace VirtualRobot::hemisphere
return jacobian;
}
- Eigen::Vector2d Joint::angleToPosition(const Eigen::Vector2d& alpha) const
+
+ Joint::ActuatorPosition Joint::angleToPosition(const Joint::ActuatorAngle& 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
index 7d2bda4861ccf44c13f02d513a3f38378d6eafd1..56fa0c3c199b96e532cea65fcb5ea6bf2a031fd5 100644
--- a/VirtualRobot/Nodes/HemisphereJoint/Joint.h
+++ b/VirtualRobot/Nodes/HemisphereJoint/Joint.h
@@ -8,10 +8,19 @@
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 Joint
{
public:
+ using ActuatorPosition = Eigen::Vector2d;
+ using ActuatorAngle = Eigen::Vector2d;
using Jacobian = Eigen::Matrix<double, 6, 2>;
public:
@@ -23,18 +32,19 @@ namespace VirtualRobot::hemisphere
void setConstants(double lever, double theta0);
- void computeFkOfAngle(const Eigen::Vector2d& alpha12);
+ void computeFkOfAngle(const ActuatorAngle& alpha12);
- void computeFkOfPosition(const Eigen::Vector2d& p12);
+ 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;
- Eigen::Vector2d angleToPosition(const Eigen::Vector2d& alpha) const;
+ ActuatorPosition angleToPosition(const ActuatorAngle& alpha) const;
public:
diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
index 38e672841b0411295b0aa76d945ec60907474f6b..86a9fe6ffc6cf3bbc8500dec3697b744625b8099 100644
--- a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
+++ b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
@@ -109,8 +109,6 @@ namespace VirtualRobot
void RobotNodeHemisphere::setXmlInfo(const XmlInfo& info)
{
- this->xmlInfo = info;
-
VR_ASSERT(second.has_value());
switch (info.role)
{
@@ -265,10 +263,10 @@ namespace VirtualRobot
ReadLockPtr lock = getRobot()->getReadLock();
Physics physics = this->physics.scale(scaling);
- RobotNodeHemispherePtr result;
+ RobotNodeHemispherePtr cloned;
if (optionalDHParameter.isSet)
{
- result.reset(new RobotNodeHemisphere(
+ cloned.reset(new RobotNodeHemisphere(
newRobot, name, jointLimitLo, jointLimitHi,
optionalDHParameter.aMM() * scaling,
optionalDHParameter.dMM() * scaling,
@@ -282,7 +280,7 @@ namespace VirtualRobot
{
Eigen::Matrix4f localTransform = getLocalTransformation();
simox::math::position(localTransform) *= scaling;
- result.reset(new RobotNodeHemisphere(
+ cloned.reset(new RobotNodeHemisphere(
newRobot, name,
jointLimitLo, jointLimitHi,
localTransform, Eigen::Vector3f::Zero(),
@@ -290,12 +288,18 @@ namespace VirtualRobot
jointValueOffset, physics, colChecker, nodeType));
}
- if(xmlInfo)
+ if (this->first)
+ {
+ // We can just copy the math object.
+ cloned->first = first;
+ }
+ else if (this->second)
{
- result->setXmlInfo(xmlInfo.value());
+ cloned->second.emplace(Second{});
+ // initialize() takes care of hooking up the second to the first.
}
- return result;
+ return cloned;
}
diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.h b/VirtualRobot/Nodes/RobotNodeHemisphere.h
index 29dac84bec20133532f8a11527f89958beef5333..91e32bd11ac6ccc7c90461ce9dacd19860746e06 100644
--- a/VirtualRobot/Nodes/RobotNodeHemisphere.h
+++ b/VirtualRobot/Nodes/RobotNodeHemisphere.h
@@ -140,16 +140,15 @@ 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:
+ public:
struct JointMath
{
@@ -183,9 +182,6 @@ namespace VirtualRobot
};
std::optional<Second> second;
-
- std::optional<XmlInfo> xmlInfo;
-
};
} // namespace VirtualRobot
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>