diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index e61d4a1083ac3d56afbbea94a491a11f8cd78295..71ea62709c23258056416f3e1cde4f4ebdcb564e 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -239,6 +239,7 @@ SET(SOURCES
IK/platform/OmniWheelPlatformKinematics.cpp
IK/platform/MecanumPlatformKinematics.cpp
+ IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.cpp
Import/RobotImporterFactory.cpp
Import/SimoxXMLFactory.cpp
@@ -346,6 +347,7 @@ SET(SOURCES
Workspace/Manipulability.cpp
Workspace/Reachability.cpp
+ Workspace/NaturalPosture.cpp
Workspace/WorkspaceDataArray.cpp
Workspace/WorkspaceGrid.cpp
Workspace/WorkspaceRepresentation.cpp
@@ -453,6 +455,7 @@ SET(INCLUDES
IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h
IK/CompositeDiffIK/Soechting.h
IK/CompositeDiffIK/SoechtingNullspaceGradient.h
+ IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.h
IK/platform/OmniWheelPlatformKinematics.h
IK/platform/MecanumPlatformKinematics.h
@@ -590,6 +593,7 @@ SET(INCLUDES
Workspace/Manipulability.h
Workspace/Reachability.h
+ Workspace/NaturalPosture.h
Workspace/VoxelTree6D.hpp
Workspace/VoxelTree6DElement.hpp
Workspace/VoxelTreeND.hpp
diff --git a/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.cpp b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9ebfe5e0d1e833b6386d3db844a730854a02c702
--- /dev/null
+++ b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.cpp
@@ -0,0 +1,178 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* @author Fabian Reister (fabian dot reister at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "SoechtingNullspaceGradientWithWrist.h"
+
+#include <cmath>
+
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
+
+#include "MathTools.h"
+#include "Soechting.h"
+#include "VirtualRobot.h"
+
+namespace VirtualRobot
+{
+ SoechtingNullspaceGradientWithWrist::SoechtingNullspaceGradientWithWrist(
+ const CompositeDiffIK::TargetPtr& target,
+ const std::string& shoulderName,
+ const Soechting::ArmType& arm,
+ const ArmJointsWithWrist& joints) :
+ CompositeDiffIK::NullspaceGradient(joints.getRobotNodeNames()),
+ rns(joints.createRobotNodeSet("SoechtingWithWrist" + std::to_string(arm))),
+ target(target),
+ shoulder(rns->getRobot()->getRobotNode(shoulderName)),
+ arm(arm),
+ joints(joints)
+ {
+ }
+
+ void SoechtingNullspaceGradientWithWrist::init(CompositeDiffIK::Parameters&)
+ {
+ // Do nothing
+ }
+
+ Eigen::VectorXf
+ SoechtingNullspaceGradientWithWrist::getGradient(CompositeDiffIK::Parameters& params,
+ int /*stepNr*/)
+ {
+ const size_t dim = rns->getSize();
+
+ auto sa = calcShoulderAngles(params);
+ Eigen::VectorXf weights = Eigen::VectorXf::Zero(dim);
+ Eigen::VectorXf target = Eigen::VectorXf::Zero(dim);
+
+ const auto set = [&](const auto& joint, const float w, const float t)
+ {
+ if (joint)
+ {
+ auto id = rns->getRobotNodeIndex(joint);
+ VR_ASSERT(id >= 0);
+ weights[id] = w;
+ target[id] = t;
+ }
+ };
+
+ set(joints.clavicula, 0.5f, sa.C);
+ set(joints.shoulder1, 1, sa.SE);
+ set(joints.shoulder2, 2, sa.SR);
+ set(joints.shoulder3, 0.5f, -M_PI / 4);
+ set(joints.elbow, 0.5f, sa.E);
+
+ set(joints.forearm, 0.5f, 0.F);
+
+ set(joints.wristAdduction, 0.5f, MathTools::deg2rad(10));
+
+ switch (arm)
+ {
+ case Soechting::ArmType::Left:
+ set(joints.wristExtension, 0.5F, -MathTools::deg2rad(20));
+ break;
+ case Soechting::ArmType::Right:
+ set(joints.wristExtension, 0.5F, +MathTools::deg2rad(20));
+ break;
+ }
+
+ // std::cout << "target" << target << std::endl;
+ // std::cout << "diff" << (target - rns->getJointValuesEigen()) << std::endl;
+
+ return (target - rns->getJointValuesEigen()).cwiseProduct(weights);
+ }
+
+ SoechtingNullspaceGradientWithWrist::ShoulderAngles
+ SoechtingNullspaceGradientWithWrist::calcShoulderAngles(
+ const CompositeDiffIK::Parameters& /*params*/) const
+ {
+ const Eigen::Matrix4f currentShoulder = shoulder->getPoseInRootFrame();
+
+ Soechting::Arm arm;
+ arm.armType = this->arm;
+ arm.shoulder = currentShoulder.block<3, 1>(0, 3);
+ arm.upperArmLength = 0.F;
+ arm.forearmLength = 0.F;
+
+ Eigen::Vector3f targetPosition = target->target.block<3, 1>(0, 3);
+ const auto sa = Soechting::CalculateAngles(targetPosition, arm, 1.3, false);
+
+ const Eigen::AngleAxisf aaSE(sa.SE, Eigen::Vector3f::UnitX());
+ const Eigen::AngleAxisf aaSY(-sa.SY, Eigen::Vector3f::UnitZ());
+ const Eigen::AngleAxisf aaEE(-sa.EE, Eigen::Vector3f::UnitX());
+ const Eigen::AngleAxisf aaEY(-sa.EY, Eigen::Vector3f::UnitZ());
+
+ const Eigen::Vector3f elb = Eigen::AngleAxisf(-sa.SE, Eigen::Vector3f::UnitX()) * aaSY *
+ aaSE * -Eigen::Vector3f::UnitZ();
+ const float SR = std::atan2(elb(0), -elb(2));
+
+ ShoulderAngles res;
+ res.SR = std::max(-0.1F, SR);
+ res.SE = sa.SE;
+ res.E = sa.EE;
+ res.C = 0;
+ return res;
+ }
+
+ RobotNodeSetPtr SoechtingNullspaceGradientWithWrist::ArmJointsWithWrist::createRobotNodeSet(
+ const std::string& name) const
+ {
+ std::vector<RobotNodePtr> robotNodes;
+ for (const auto& node : {clavicula,
+ shoulder1,
+ shoulder2,
+ shoulder3,
+ elbow,
+ forearm,
+ wristAdduction,
+ wristExtension})
+ {
+ if (node)
+ {
+ robotNodes.push_back(node);
+ }
+ }
+ if (robotNodes.empty())
+ {
+ return nullptr;
+ }
+
+ auto frontNode = robotNodes.front();
+ return RobotNodeSet::createRobotNodeSet(frontNode->getRobot(), name, robotNodes, frontNode);
+ }
+
+ std::vector<std::string>
+ SoechtingNullspaceGradientWithWrist::ArmJointsWithWrist::getRobotNodeNames() const
+ {
+ std::vector<std::string> nodeNames;
+ for (const auto& node : {clavicula,
+ shoulder1,
+ shoulder2,
+ shoulder3,
+ elbow,
+ forearm,
+ wristAdduction,
+ wristExtension})
+ {
+ if (node)
+ {
+ nodeNames.push_back(node->getName());
+ }
+ }
+ return nodeNames;
+ }
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.h b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.h
new file mode 100644
index 0000000000000000000000000000000000000000..22fc81410bab425aa1801c06a8e2b767f5465534
--- /dev/null
+++ b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.h
@@ -0,0 +1,73 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* @author Fabian Reister (fabian dot reister at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include "CompositeDiffIK.h"
+#include "Soechting.h"
+
+namespace VirtualRobot
+{
+
+ class SoechtingNullspaceGradientWithWrist : public CompositeDiffIK::NullspaceGradient
+ {
+ public:
+ struct ArmJointsWithWrist
+ {
+ RobotNodePtr clavicula = nullptr;
+ RobotNodePtr shoulder1 = nullptr;
+ RobotNodePtr shoulder2 = nullptr;
+ RobotNodePtr shoulder3 = nullptr;
+ RobotNodePtr elbow = nullptr;
+
+ RobotNodePtr forearm = nullptr;
+
+ RobotNodePtr wristAdduction = nullptr;
+ RobotNodePtr wristExtension = nullptr;
+
+ RobotNodeSetPtr createRobotNodeSet(const std::string& name) const;
+ std::vector<std::string> getRobotNodeNames() const;
+ };
+
+ struct ShoulderAngles
+ {
+ float SE, SR, E, C;
+ };
+
+ SoechtingNullspaceGradientWithWrist(const CompositeDiffIK::TargetPtr& target,
+ const std::string& shoulderName,
+ const Soechting::ArmType& arm,
+ const ArmJointsWithWrist& joints);
+ virtual ~SoechtingNullspaceGradientWithWrist() = default;
+
+ void init(CompositeDiffIK::Parameters&) override;
+ Eigen::VectorXf getGradient(CompositeDiffIK::Parameters& params, int stepNr) override;
+
+ RobotNodeSetPtr rns;
+ CompositeDiffIK::TargetPtr target;
+ VirtualRobot::RobotNodePtr shoulder;
+ Soechting::ArmType arm;
+ ArmJointsWithWrist joints;
+
+ ShoulderAngles calcShoulderAngles(const CompositeDiffIK::Parameters& params) const;
+ };
+
+ using SoechtingNullspaceGradientWithWristPtr =
+ std::shared_ptr<SoechtingNullspaceGradientWithWrist>;
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/Workspace/NaturalPosture.cpp b/VirtualRobot/Workspace/NaturalPosture.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8de6606410fade85f169b01ed780f9327766a001
--- /dev/null
+++ b/VirtualRobot/Workspace/NaturalPosture.cpp
@@ -0,0 +1,290 @@
+#include "NaturalPosture.h"
+
+#include <cfloat>
+#include <climits>
+#include <cmath>
+#include <fstream>
+#include <stdexcept>
+
+#include <Eigen/src/Core/Matrix.h>
+
+#include <VirtualRobot/IK/CompositeDiffIK/Soechting.h>
+#include <VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h>
+
+#include "../Grasping/Grasp.h"
+#include "../Grasping/GraspSet.h"
+#include "../ManipulationObject.h"
+#include "../Robot.h"
+#include "../RobotNodeSet.h"
+#include "../VirtualRobotException.h"
+#include "IK/CompositeDiffIK/CompositeDiffIK.h"
+#include "IK/CompositeDiffIK/SoechtingNullspaceGradient.h"
+#include "IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.h"
+#include "VirtualRobot.h"
+
+namespace VirtualRobot
+{
+
+ NaturalPosture::NaturalPosture(RobotPtr robot) : WorkspaceRepresentation(robot)
+ {
+ type = "NaturalPosture";
+ }
+
+ void NaturalPosture::customInitialize()
+ {
+ // auto target1 = ik->addTarget(robot->getRobotNode(tcpNode->getName()), VirtualRobot::IKSolver::All);
+ }
+
+ float NaturalPosture::evaluate()
+ {
+ SoechtingNullspaceGradientWithWristPtr soechtingNullspaceGradient;
+
+ CompositeDiffIK::Parameters params;
+
+ const CompositeDiffIK::TargetPtr target(new CompositeDiffIK::Target(
+ tcpNode, tcpNode->getGlobalPose(), VirtualRobot::IKSolver::All));
+
+ if (robot->getName() == "Armar6" && nodeSet->getName() == "RightArm")
+ {
+ // std::cout << "Adding soechting nullspace" << std::endl;
+ VirtualRobot::SoechtingNullspaceGradientWithWrist::ArmJointsWithWrist armjoints;
+ armjoints.clavicula = robot->getRobotNode("ArmR1_Cla1");
+ armjoints.shoulder1 = robot->getRobotNode("ArmR2_Sho1");
+ armjoints.shoulder2 = robot->getRobotNode("ArmR3_Sho2");
+ armjoints.shoulder3 = robot->getRobotNode("ArmR4_Sho3");
+ armjoints.elbow = robot->getRobotNode("ArmR5_Elb1");
+ armjoints.forearm = robot->getRobotNode("ArmR6_Elb2");
+ armjoints.wristAdduction = robot->getRobotNode("ArmR7_Wri1");
+ armjoints.wristExtension = robot->getRobotNode("ArmR8_Wri2");
+
+ soechtingNullspaceGradient.reset(new VirtualRobot::SoechtingNullspaceGradientWithWrist(
+ target, "ArmR2_Sho1", VirtualRobot::Soechting::ArmType::Right, armjoints));
+ soechtingNullspaceGradient->kP = 1.0;
+ // ik->addNullspaceGradient(gradient);
+ }
+ else if (robot->getName() == "Armar6" && nodeSet->getName() == "LeftArm")
+ {
+ // std::cout << "Adding soechting nullspace" << std::endl;
+ VirtualRobot::SoechtingNullspaceGradientWithWrist::ArmJointsWithWrist armjoints;
+ armjoints.clavicula = robot->getRobotNode("ArmL1_Cla1");
+ armjoints.shoulder1 = robot->getRobotNode("ArmL2_Sho1");
+ armjoints.shoulder2 = robot->getRobotNode("ArmL3_Sho2");
+ armjoints.shoulder3 = robot->getRobotNode("ArmL4_Sho3");
+ armjoints.elbow = robot->getRobotNode("ArmL5_Elb1");
+ armjoints.forearm = robot->getRobotNode("ArmL6_Elb2");
+ armjoints.wristAdduction = robot->getRobotNode("ArmL7_Wri1");
+ armjoints.wristExtension = robot->getRobotNode("ArmL8_Wri2");
+
+ soechtingNullspaceGradient.reset(new VirtualRobot::SoechtingNullspaceGradientWithWrist(
+ target, "ArmL2_Sho1", VirtualRobot::Soechting::ArmType::Left, armjoints));
+ soechtingNullspaceGradient->kP = 1.0;
+
+ // ik->addNullspaceGradient(gradient);
+ }
+ else
+ {
+ throw std::runtime_error("Unknown robot");
+ }
+
+ const auto weightedDiff = soechtingNullspaceGradient->getGradient(params, -1);
+ const float e1 = weightedDiff.squaredNorm();
+
+ // auto wristAdduction = nodeSet->getNode("ArmR7_Wri1");
+ // auto wristExtension = nodeSet->getNode("ArmR8_Wri2");
+
+ // const Eigen::Vector2f wristTarget{M_PI_2f32 + MathTools::deg2rad(10),
+ // M_PI_2f32 - MathTools::deg2rad(20)};
+ // const Eigen::Vector2f wristJointValues{wristAdduction->getJointValue(),
+ // wristExtension->getJointValue()};
+
+ // set(wristAdduction, 1.F, M_PI_2f32 + MathTools::deg2rad(10));
+ // set(wristExtension, 1.F, M_PI_2f32 - MathTools::deg2rad(20));
+
+ // const float e2 = (wristTarget - wristJointValues).squaredNorm();
+
+ return std::sqrt(e1);
+ }
+
+ void NaturalPosture::addPose(const Eigen::Matrix4f& pose)
+ {
+ // VR_INFO << "Adding pose";
+
+ Eigen::Matrix4f p = pose;
+ toLocal(p);
+
+ float x[6];
+
+ matrix2Vector(p, x);
+ //MathTools::eigen4f2rpy(p,x);
+
+ // check for achieved values
+ for (int i = 0; i < 6; i++)
+ {
+ if (x[i] < achievedMinValues[i])
+ {
+ achievedMinValues[i] = x[i];
+ }
+
+ if (x[i] > achievedMaxValues[i])
+ {
+ achievedMaxValues[i] = x[i];
+ }
+ }
+
+ // get voxels
+ unsigned int v[6];
+
+ if (getVoxelFromPose(x, v))
+ {
+
+ // float m = getCurrentManipulability(qualMeasure, selfDistSt, selfDistDyn);
+ // float mSc = m / maxManip;
+
+ // if (mSc > 1)
+ // {
+ // if (mSc > 1.05)
+ // {
+ // VR_WARNING << "Manipulability is larger than max value. Current Manip:" << m << ", maxManip:" << maxManip << ", percent:" << mSc << std::endl;
+ // }
+
+ // mSc = 1.0f;
+ // }
+
+ // if (m < 0)
+ // {
+ // mSc = 0;
+ // }
+
+ // unsigned char e = (unsigned char)(mSc * (float)UCHAR_MAX + 0.5f);
+
+ //cout<<"m = "<<m<<endl;
+ //cout<<"mSc = "<<mSc<<endl;
+ //cout<<"e = "<<int(e)<<endl;
+
+ const float ee = evaluate();
+
+ // VR_INFO << "evaluate: " << ee;
+
+ const float mSc = std::min(ee / 10, 1.0F);
+
+ if (mSc > 1.0)
+ {
+ VR_WARNING << "mSc too large " << mSc;
+ }
+
+ // // add at least 1, since the pose is reachable
+ // if (e == 0)
+ // {
+ // e = 1;
+ // }
+
+ const auto e = static_cast<unsigned char>(mSc * static_cast<float>(UCHAR_MAX) + 0.5F);
+
+ const auto oldVal = data->get(v);
+ if (oldVal == 0) // if unset
+ {
+ data->setDatum(v, e);
+ }
+ else
+ {
+ if (e < oldVal)
+ {
+ data->setDatum(v, e);
+ }
+ }
+ }
+ else
+ {
+ VR_WARNING << "Could not get voxel from pose!";
+ }
+
+ buildUpLoops++;
+ }
+
+ /*
+ void NaturalPosture::addRandomTCPPoses(unsigned int loops, bool checkForSelfCollisions)
+ {
+ THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "NaturalPosture data not initialized");
+
+ std::vector<float> c;
+ nodeSet->getJointValues(c);
+ bool visuSate = robot->getUpdateVisualizationStatus();
+ robot->setUpdateVisualization(false);
+
+ for (unsigned int i = 0; i < loops; i++)
+ {
+ if (setRobotNodesToRandomConfig(nodeSet, checkForSelfCollisions))
+ {
+ addCurrentTCPPose();
+ }
+ else
+ {
+ VR_WARNING << "Could not find collision-free configuration...";
+ }
+ }
+
+ robot->setUpdateVisualization(visuSate);
+ robot->setJointValues(nodeSet, c);
+ }*/
+
+ // bool NaturalPosture::isReachable(const Eigen::Matrix4f& globalPose)
+ // {
+ // return isCovered(globalPose);
+ // }
+
+ // VirtualRobot::GraspSetPtr NaturalPosture::getReachableGrasps(GraspSetPtr grasps, ManipulationObjectPtr object)
+ // {
+ // THROW_VR_EXCEPTION_IF(!object, "no object");
+ // THROW_VR_EXCEPTION_IF(!grasps, "no grasps");
+
+ // GraspSetPtr result(new GraspSet(grasps->getName(), grasps->getRobotType(), grasps->getEndEffector()));
+
+ // for (unsigned int i = 0; i < grasps->getSize(); i++)
+ // {
+ // Eigen::Matrix4f m = grasps->getGrasp(i)->getTcpPoseGlobal(object->getGlobalPose());
+
+ // if (isReachable(m))
+ // {
+ // result->addGrasp(grasps->getGrasp(i));
+ // }
+ // }
+
+ // return result;
+ // }
+
+ // Eigen::Matrix4f NaturalPosture::sampleReachablePose()
+ // {
+ // return sampleCoveredPose();
+ // }
+
+ // VirtualRobot::WorkspaceRepresentationPtr NaturalPosture::clone()
+ // {
+ // VirtualRobot::NaturalPosturePtr res(new NaturalPosture(robot));
+ // res->setOrientationType(this->orientationType);
+ // res->versionMajor = this->versionMajor;
+ // res->versionMinor = this->versionMinor;
+ // res->nodeSet = this->nodeSet;
+ // res->type = this->type;
+
+ // res->baseNode = this->baseNode;
+ // res->tcpNode = this->tcpNode;
+ // res->staticCollisionModel = this->staticCollisionModel;
+ // res->dynamicCollisionModel = this->dynamicCollisionModel;
+ // res->buildUpLoops = this->buildUpLoops;
+ // res->collisionConfigs = this->collisionConfigs;
+ // res->discretizeStepTranslation = this->discretizeStepTranslation;
+ // res->discretizeStepRotation = this->discretizeStepRotation;
+ // memcpy(res->minBounds, this->minBounds, sizeof(float) * 6);
+ // memcpy(res->maxBounds, this->maxBounds, sizeof(float) * 6);
+ // memcpy(res->numVoxels, this->numVoxels, sizeof(float) * 6);
+ // memcpy(res->achievedMinValues, this->achievedMinValues, sizeof(float) * 6);
+ // memcpy(res->achievedMaxValues, this->achievedMaxValues, sizeof(float) * 6);
+ // memcpy(res->spaceSize, this->spaceSize, sizeof(float) * 6);
+
+ // res->adjustOnOverflow = this->adjustOnOverflow;
+ // res->data.reset(this->data->clone());
+
+ // return res;
+ // }
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/Workspace/NaturalPosture.h b/VirtualRobot/Workspace/NaturalPosture.h
new file mode 100644
index 0000000000000000000000000000000000000000..ca1e335cfea2b593b52850625743428b1c5a8036
--- /dev/null
+++ b/VirtualRobot/Workspace/NaturalPosture.h
@@ -0,0 +1,77 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Peter Kaiser, Nikolaus Vahrenkamp
+* @copyright 2011 Peter Kaiser, Nikolaus Vahrenkamp
+* GNU Lesser General Public License
+*
+*/
+#pragma once
+
+#include "../VirtualRobot.h"
+#include "WorkspaceRepresentation.h"
+
+#include <VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h>
+
+namespace VirtualRobot
+{
+
+
+ class VIRTUAL_ROBOT_IMPORT_EXPORT NaturalPosture : public WorkspaceRepresentation, public std::enable_shared_from_this<NaturalPosture>
+ {
+ public:
+ friend class CoinVisualizationFactory;
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ NaturalPosture(RobotPtr robot);
+
+ void customInitialize() override;
+
+
+ void addPose(const Eigen::Matrix4f& pose) override;
+
+ // /*!
+ // Returns true, if the corresponding NaturalPosture entry is non zero.
+ // */
+ // bool isReachable(const Eigen::Matrix4f& globalPose);
+
+ // /*!
+ // Returns all reachable grasps that can be applied at the current position of object.
+ // */
+ // GraspSetPtr getReachableGrasps(GraspSetPtr grasps, ManipulationObjectPtr object);
+
+
+ // //! returns a random pose that is covered by the workspace data.
+ // Eigen::Matrix4f sampleReachablePose();
+
+ // /*!
+ // Creates a deep copy of this data structure. A NaturalPosturePtr is returned.
+ // */
+ // WorkspaceRepresentationPtr clone() override;
+
+
+
+ protected:
+
+ float evaluate();
+
+ };
+
+
+} // namespace VirtualRobot
+