diff --git a/VirtualRobot/IK/DifferentialIK.cpp b/VirtualRobot/IK/DifferentialIK.cpp
index b70bba190863d7a20eb606f6fe7ec7daef499774..23f6b730846254277032f61eafcb1307ce698aee 100644
--- a/VirtualRobot/IK/DifferentialIK.cpp
+++ b/VirtualRobot/IK/DifferentialIK.cpp
@@ -390,11 +390,12 @@ namespace VirtualRobot
// Calculus for rotational joints is different as for prismatic joints.
if (dof->isRotationalJoint())
{
- // get axis
+ // todo: find a better way of handling different joint types
std::shared_ptr<RobotNodeRevolute> revolute
= std::dynamic_pointer_cast<RobotNodeRevolute>(dof);
THROW_VR_EXCEPTION_IF(!revolute, "Internal error: expecting revolute joint");
- // todo: find a better way of handling different joint types
+
+ // get axis
axis = revolute->getJointRotationAxis(coordSystem);
// if necessary calculate the position part of the Jacobian
@@ -452,18 +453,31 @@ namespace VirtualRobot
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>();
+ using Matrix3x2f = Eigen::Matrix<float, 3, 2>;
+
+ const Matrix3x2f velocities = jacobian.block<3, 2>(0, 0).cast<float>();
+ const Matrix3x2f rotAxes = jacobian.block<3, 2>(3, 0).cast<float>();
+
+ const Eigen::Matrix3f globalOri = second.firstNode->getGlobalOrientation();
+
+ Matrix3x2f velocitiesCoord = globalOri * velocities;
+ Matrix3x2f rotAxesCoord = globalOri * rotAxes;
+ if (coordSystem)
+ {
+ auto coordSystemInverseGlobalRot = coordSystem->getGlobalOrientation().transpose();
+
+ velocitiesCoord = coordSystemInverseGlobalRot * velocitiesCoord;
+ rotAxesCoord = coordSystemInverseGlobalRot * rotAxesCoord;
+ }
+
+ tmpUpdateJacobianPosition.block<3, 2>(0, i - 1) = velocitiesCoord;
+ tmpUpdateJacobianOrientation.block<3, 2>(0, i - 1) = rotAxesCoord;
}
else
{
diff --git a/VirtualRobot/Nodes/RobotNodeFourBar.cpp b/VirtualRobot/Nodes/RobotNodeFourBar.cpp
index a70581dd9fec393485ec191d9d01234d0a67c417..615a468dc5f35b6d85178b368cf350a4cab1f5d4 100644
--- a/VirtualRobot/Nodes/RobotNodeFourBar.cpp
+++ b/VirtualRobot/Nodes/RobotNodeFourBar.cpp
@@ -176,7 +176,7 @@ namespace VirtualRobot
{
this->xmlInfo = info;
- VR_ASSERT(second.has_value());
+ VR_ASSERT(active.has_value());
switch (info.role)
{
case Role::PASSIVE:
@@ -199,16 +199,16 @@ namespace VirtualRobot
bool
RobotNodeFourBar::initialize(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(first.has_value() xor active.has_value(),
+ std::stringstream() << first.has_value() << " / " << active.has_value());
- // The second node needs to store a reference to the first node.
+ // The active node needs to store a reference to the first node.
// Whenever the joint value has changed, the passive joint will be updated.
if (active)
{
std::cout << "Initializing active four bar joint" << std::endl;
- VR_ASSERT_MESSAGE(not second->first, "Second must not be initialized yet.");
+ VR_ASSERT_MESSAGE(not active->passive, "Second must not be initialized yet.");
VirtualRobot::SceneObjectPtr currentParent = parent;
@@ -281,8 +281,8 @@ namespace VirtualRobot
void
RobotNodeFourBar::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(first.has_value() xor active.has_value(),
+ std::stringstream() << first.has_value() << " / " << active.has_value());
std::cout << "Updating RobotNodeFourBar::updateTransformationMatrices" << std::endl;
@@ -316,7 +316,7 @@ namespace VirtualRobot
// }
// else if (active)
// {
- // VR_ASSERT_MESSAGE(second->first, "First node must be known to second node.");
+ // VR_ASSERT_MESSAGE(active->first, "First node must be known to active node.");
// JointMath& math = active->math();
// Eigen::Vector2f actuators(active->passive->getJointValue(), this->getJointValue());
@@ -333,7 +333,7 @@ namespace VirtualRobot
// Eigen::IOFormat iof(5, 0, " ", "\n", " [", "]");
// std::cout
- // << __FUNCTION__ << "() of second actuator with "
+ // << __FUNCTION__ << "() of active actuator with "
// << "\n lever = " << math.joint.lever << "\n theta0 = " << math.joint.theta0
// << "\n radius = " << math.joint.radius << "\n joint value = " << jointValue
// << "\n actuator (angle) = \n"
@@ -351,8 +351,8 @@ namespace VirtualRobot
RobotNodeFourBar::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(first.has_value() xor active.has_value(),
+ std::stringstream() << first.has_value() << " / " << active.has_value());
if (printDecoration)
{
@@ -367,7 +367,7 @@ namespace VirtualRobot
}
else if (active)
{
- std::cout << "* four_bar joint second node";
+ std::cout << "* four_bar joint active node";
// std::cout << "* Transform: \n"
// << active->math.joint.getEndEffectorTransform() << std::endl;
}
@@ -519,8 +519,8 @@ namespace VirtualRobot
std::string
RobotNodeFourBar::_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(first.has_value() xor active.has_value(),
+ std::stringstream() << first.has_value() << " / " << active.has_value());
if (first)
{
diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
index a2d999867affb1b8adf6f5d79ffc12675ef5607b..d472654467060a64d9de011afd00050be2575c48 100644
--- a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
+++ b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
@@ -127,7 +127,7 @@ namespace VirtualRobot
// The second node needs to store a reference to the first node.
if (secondData)
{
- VR_ASSERT_MESSAGE(not secondData->first, "Second must not be initialized yet.");
+ VR_ASSERT_MESSAGE(not secondData->firstNode, "Second must not be initialized yet.");
RobotNodeHemisphere* firstNode = dynamic_cast<RobotNodeHemisphere*>(parent.get());
RobotNodeHemisphere* secondNode = this;
@@ -171,7 +171,7 @@ namespace VirtualRobot
}
else if (secondData)
{
- VR_ASSERT_MESSAGE(secondData->first, "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();
Eigen::Vector2f actuators(secondData->firstNode->getJointValue(), this->getJointValue());