diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 6c6ea014be4e39b912e94ac6ff19b61959d3559e..0e6ea498e694f75c9541f9c5e18334ec1f27ba3f 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -13,6 +13,7 @@
<Variant baseType="::armarx::LinkedDirectionBase" dataType="::armarx::LinkedDirection" humanName="LinkedDirection" include="RobotAPI/libraries/core/LinkedPose.h" />
<Variant baseType="::armarx::OrientedPointBase" dataType="::armarx::OrientedPoint" humanName="OrientedPoint" include="RobotAPI/libraries/core/OrientedPoint.h" />
<Variant baseType="::armarx::FramedOrientedPointBase" dataType="::armarx::FramedOrientedPoint" humanName="FramedOrientedPoint" include="RobotAPI/libraries/core/FramedOrientedPoint.h" />
+ <Variant baseType="::armarx::CartesianPositionControllerConfigBase" dataType="::armarx::CartesianPositionControllerConfig" humanName="CartesianPositionControllerConfig" include="RobotAPI/libraries/core/CartesianPositionController.h" />
<Variant baseType="::armarx::TrajectoryBase" dataType="::armarx::Trajectory" humanName="Trajectory" include="RobotAPI/libraries/core/Trajectory.h"/>
</Lib>
<Lib name="RobotAPIInterfaces">
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.cpp b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
index 467b7b6e180b361294d6a9895abff3edafa63fa6..84ec17f805d3b31e3e3eb7a61a96a4eaae215878 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.cpp
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
@@ -63,23 +63,23 @@ PropertyDefinitionsPtr GraspCandidateObserver::createPropertyDefinitions()
bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& filter, const std::string& providerName, const GraspCandidatePtr& candidate)
{
- if(filter->providerName != "*" && filter->providerName != providerName)
+ if (filter->providerName != "*" && filter->providerName != providerName)
{
return false;
}
- if(filter->minimumSuccessProbability > candidate->graspSuccessProbability)
+ if (filter->minimumSuccessProbability > candidate->graspSuccessProbability)
{
return false;
}
- if(filter->approach != AnyApproach && (candidate->executionHints == 0 || filter->approach != candidate->executionHints->approach))
+ if (filter->approach != AnyApproach && (candidate->executionHints == 0 || filter->approach != candidate->executionHints->approach))
{
return false;
}
- if(filter->preshape != AnyAperture && (candidate->executionHints == 0 || filter->preshape != candidate->executionHints->preshape))
+ if (filter->preshape != AnyAperture && (candidate->executionHints == 0 || filter->preshape != candidate->executionHints->preshape))
{
return false;
}
- if(filter->objectType != AnyObject && filter->objectType != candidate->objectType)
+ if (filter->objectType != AnyObject && filter->objectType != candidate->objectType)
{
return false;
}
@@ -182,7 +182,7 @@ GraspCandidateSeq GraspCandidateObserver::getAllCandidates(const Ice::Current&)
return all;
}
-GraspCandidateSeq GraspCandidateObserver::getCandidatesByProvider(const std::string& providerName, const Ice::Current&c)
+GraspCandidateSeq GraspCandidateObserver::getCandidatesByProvider(const std::string& providerName, const Ice::Current& c)
{
CandidateFilterConditionPtr filter = new CandidateFilterCondition();
filter->providerName = providerName;
@@ -219,14 +219,14 @@ IntMap GraspCandidateObserver::getAllUpdateCounters(const Ice::Current& provider
return updateCounters;
}
-bool GraspCandidateObserver::setProviderConfig(const std::string& providerName, const ConfigMap& config, const Ice::Current&)
+bool GraspCandidateObserver::setProviderConfig(const std::string& providerName, const StringVariantBaseMap& config, const Ice::Current&)
{
ScopedLock lock(dataMutex);
if (providers.count(providerName) == 0)
{
return false;
}
- configTopic->setGraspCandidateGenerationConfig(providerName, config);
+ configTopic->setServiceConfig(providerName, config);
return true;
}
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.h b/source/RobotAPI/components/units/GraspCandidateObserver.h
index ade39c3773c9f794b1061ded535a8a4317f48c96..f038c505542fc70e7fe3a1195415c1d0a5489320 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.h
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.h
@@ -84,11 +84,11 @@ namespace armarx
grasping::ProviderInfoPtr getProviderInfo(const std::string& providerName, const Ice::Current&) override;
bool hasProvider(const std::string& providerName, const Ice::Current& c) override;
grasping::GraspCandidateSeq getAllCandidates(const Ice::Current&) override;
- grasping::GraspCandidateSeq getCandidatesByProvider(const std::string& providerName, const Ice::Current&c) override;
+ grasping::GraspCandidateSeq getCandidatesByProvider(const std::string& providerName, const Ice::Current& c) override;
grasping::GraspCandidateSeq getCandidatesByFilter(const grasping::CandidateFilterConditionPtr& filter, const Ice::Current&) override;
Ice::Int getUpdateCounterByProvider(const std::string& providerName, const Ice::Current&) override;
grasping::IntMap getAllUpdateCounters(const Ice::Current& providerName) override;
- bool setProviderConfig(const std::string& providerName, const grasping::ConfigMap& config, const Ice::Current&) override;
+ bool setProviderConfig(const std::string& providerName, const StringVariantBaseMap& config, const Ice::Current&) override;
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 8250c363f3a4b1e3108bf5fa9306c938ec1510f7..a170a4b5672088d17c85b4b09fd4df9b00c19b25 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -18,6 +18,7 @@ set(SLICE_FILES
core/RobotStateObserverInterface.ice
core/Trajectory.ice
core/CartesianSelection.ice
+ core/CartesianPositionControllerConfig.ice
selflocalisation/SelfLocalisationProcess.ice
diff --git a/source/RobotAPI/interface/core/CartesianPositionControllerConfig.ice b/source/RobotAPI/interface/core/CartesianPositionControllerConfig.ice
new file mode 100644
index 0000000000000000000000000000000000000000..0c1418a257091bead62d86a0629967ba8bfde77b
--- /dev/null
+++ b/source/RobotAPI/interface/core/CartesianPositionControllerConfig.ice
@@ -0,0 +1,49 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX 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 General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::RobotAPI
+* @author Martin Miller
+* @copyright 2015 Humanoids Group, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/observers/Filters.ice>
+
+module armarx
+{
+ /**
+ * [CartesianPositionControllerConfigBase] defines the config for CartesianPositionController
+ */
+ class CartesianPositionControllerConfigBase extends VariantDataClass
+ {
+ float KpPos = 1;
+ float KpOri = 1;
+ float maxPosVel = -1;
+ float maxOriVel = -1;
+
+ float thresholdOrientationNear = -1;
+ float thresholdOrientationReached = -1;
+ float thresholdPositionNear = -1;
+ float thresholdPositionReached = -1;
+ };
+
+
+
+};
+
diff --git a/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice b/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice
index cdae6e66d181ebaeef6082b87151155594a068ea..5dbf43abddd30495ac7e8ab72b36d07f2179c45b 100644
--- a/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice
+++ b/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice
@@ -55,7 +55,7 @@ module armarx
GraspCandidateSeq getCandidatesByFilter(CandidateFilterCondition filter);
int getUpdateCounterByProvider(string providerName);
IntMap getAllUpdateCounters();
- bool setProviderConfig(string providerName, ConfigMap config);
+ bool setProviderConfig(string providerName, StringVariantBaseMap config);
};
};
diff --git a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
index 825b90fc87e51ffdcc2855ec532122fcabc40c6b..25d44c286f17fe43ffecad24162bd55b329a23ec 100644
--- a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
+++ b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
@@ -26,6 +26,7 @@
#include <ArmarXCore/interface/core/BasicTypes.ice>
#include <RobotAPI/interface/core/FramedPoseBase.ice>
#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/observers/RequestableService.ice>
module armarx
{
@@ -60,6 +61,11 @@ module armarx
ApertureType preshape;
ApproachType approach;
};
+ class GraspCandidateReachabilityInfo
+ {
+ bool reachable;
+ float minimumJointLimitMargin;
+ };
@@ -78,14 +84,15 @@ module armarx
GraspCandidateSourceInfo sourceInfo; // (optional)
GraspCandidateExecutionHints executionHints; // (optional)
+ GraspCandidateReachabilityInfo reachabilityInfo; // (optional)
};
- dictionary<string, VariantBase> ConfigMap;
+
class ProviderInfo
{
ObjectTypeEnum objectType = AnyObject;
- ConfigMap currentConfig;
+ StringVariantBaseMap currentConfig;
};
sequence<GraspCandidate> GraspCandidateSeq;
@@ -97,10 +104,9 @@ module armarx
void reportProviderInfo(string providerName, ProviderInfo info);
void reportGraspCandidates(string providerName, GraspCandidateSeq candidates);
};
- interface GraspCandidateProviderInterface
+
+ interface GraspCandidateProviderInterface extends RequestableServiceListenerInterface
{
- void requestGraspCandidateGeneration(string providerName, int timeoutMS);
- void setGraspCandidateGenerationConfig(string providerName, ConfigMap config);
};
};
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
index a54cf2fe86393f94a679b1d09d45cb5a7d48f292..a54284c3a03b39a3406b74d83cebae127856a815 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -51,12 +51,34 @@ PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNode
}
}
+void PositionControllerHelper::readConfig(const CartesianPositionControllerConfigBasePtr& config)
+{
+ thresholdOrientationNear = config->thresholdOrientationNear;
+ thresholdOrientationReached = config->thresholdOrientationReached;
+ thresholdPositionNear = config->thresholdPositionNear;
+ thresholdPositionReached = config->thresholdPositionReached;
+ posController.KpOri = config->KpOri;
+ posController.KpPos = config->KpPos;
+ posController.maxOriVel = config->maxOriVel;
+ posController.maxPosVel = config->maxPosVel;
+}
+
void PositionControllerHelper::update()
+{
+ updateRead();
+ updateWrite();
+}
+
+void PositionControllerHelper::updateRead()
{
if (!isLastWaypoint() && isCurrentTargetNear())
{
currentWaypointIndex++;
}
+}
+
+void PositionControllerHelper::updateWrite()
+{
Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
velocityControllerHelper->setTargetVelocity(cv + feedForwardVelocity);
if (autoClearFeedForward)
@@ -109,6 +131,15 @@ void PositionControllerHelper::clearFeedForwardVelocity()
feedForwardVelocity = Eigen::Vector6f::Zero();
}
+void PositionControllerHelper::immediateHardStop(bool clearTargets)
+{
+ velocityControllerHelper->controller->immediateHardStop();
+ if (clearTargets)
+ {
+ setTarget(posController.getTcp()->getPoseInRootFrame());
+ }
+}
+
float PositionControllerHelper::getPositionError() const
{
return posController.getPositionError(getCurrentTarget());
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
index e4012fbb5ce8c99f951eea53833d11f352571771..e7268120ab1132cf2aa7a77c4b449fd3d0a1faac 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
@@ -53,7 +53,15 @@ namespace armarx
PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<Eigen::Matrix4f>& waypoints);
PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<PosePtr>& waypoints);
+ void readConfig(const CartesianPositionControllerConfigBasePtr& config);
+
+ // read data and write targets, call this if you are unsure, anywhere in your control loop.
+ // use updateRead and updateWrite for better performance
void update();
+ // read data, call this after robot sync
+ void updateRead();
+ // write targets, call this at the end of your control loop, before the sleep
+ void updateWrite();
void setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints);
void setNewWaypoints(const std::vector<PosePtr>& waypoints);
@@ -62,6 +70,7 @@ namespace armarx
void setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity);
void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri);
void clearFeedForwardVelocity();
+ void immediateHardStop(bool clearTargets = true);
float getPositionError() const;
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index f089984659762898e5da278375c627656d8d375f..33566020ec5bb244c21719c4d9b323ae6a2896c2 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -47,6 +47,7 @@ set(LIB_FILES
CartesianVelocityRamp.cpp
JointVelocityRamp.cpp
CartesianVelocityControllerWithRamp.cpp
+ SimpleDiffIK.cpp
)
set(LIB_HEADERS
@@ -90,6 +91,7 @@ set(LIB_HEADERS
CartesianVelocityRamp.h
JointVelocityRamp.h
CartesianVelocityControllerWithRamp.h
+ SimpleDiffIK.h
EigenHelpers.h
)
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 4559e139c7d93e803f1ff7d070a77698396863a0..60c62092f4e34302e825e530436e150204f91fe5 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -100,3 +100,64 @@ Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Mat
Eigen::AngleAxisf aa(oriDir);
return aa.axis() * aa.angle();
}
+
+VirtualRobot::RobotNodePtr CartesianPositionController::getTcp() const
+{
+ return tcp;
+}
+
+
+
+#define SS_OUT(x) ss << #x << " = " << x << "\n"
+#define SET_FLT(x) obj->setFloat(#x, x)
+#define GET_FLT(x) x = obj->getFloat(#x);
+
+CartesianPositionControllerConfig::CartesianPositionControllerConfig()
+{
+}
+
+std::string CartesianPositionControllerConfig::output(const Ice::Current& c) const
+{
+ std::stringstream ss;
+
+ SS_OUT(KpPos);
+ SS_OUT(KpOri);
+ SS_OUT(maxPosVel);
+ SS_OUT(maxOriVel);
+ SS_OUT(thresholdOrientationNear);
+ SS_OUT(thresholdOrientationReached);
+ SS_OUT(thresholdPositionNear);
+ SS_OUT(thresholdPositionReached);
+
+ return ss.str();
+}
+
+void CartesianPositionControllerConfig::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
+{
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ SET_FLT(KpPos);
+ SET_FLT(KpOri);
+ SET_FLT(maxPosVel);
+ SET_FLT(maxOriVel);
+ SET_FLT(thresholdOrientationNear);
+ SET_FLT(thresholdOrientationReached);
+ SET_FLT(thresholdPositionNear);
+ SET_FLT(thresholdPositionReached);
+
+}
+
+void CartesianPositionControllerConfig::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
+{
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ GET_FLT(KpPos);
+ GET_FLT(KpOri);
+ GET_FLT(maxPosVel);
+ GET_FLT(maxOriVel);
+ GET_FLT(thresholdOrientationNear);
+ GET_FLT(thresholdOrientationReached);
+ GET_FLT(thresholdPositionNear);
+ GET_FLT(thresholdPositionReached);
+
+}
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.h b/source/RobotAPI/libraries/core/CartesianPositionController.h
index 2c4b46c5c2363938a47555ca9407a0f495e0353a..3333a45fa98a16c7dcd85ac21443a434d1bc5af1 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.h
@@ -30,6 +30,10 @@
#include <VirtualRobot/Robot.h>
#include <Eigen/Dense>
+#include <RobotAPI/interface/core/CartesianPositionControllerConfig.h>
+#include <ArmarXCore/observers/variant/Variant.h>
+#include <ArmarXCore/observers/AbstractObjectSerializer.h>
+
namespace armarx
{
class CartesianPositionController;
@@ -47,6 +51,7 @@ namespace armarx
float getOrientationError(const Eigen::Matrix4f& targetPose) const;
Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose) const;
Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose) const;
+ VirtualRobot::RobotNodePtr getTcp() const;
float KpPos = 1;
float KpOri = 1;
@@ -57,5 +62,50 @@ namespace armarx
private:
VirtualRobot::RobotNodePtr tcp;
};
+
+ namespace VariantType
+ {
+ // variant types
+ const VariantTypeId CartesianPositionControllerConfig = Variant::addTypeName("::armarx::CartesianPositionControllerConfig");
+ }
+
+ class CartesianPositionControllerConfig : virtual public CartesianPositionControllerConfigBase
+ {
+ public:
+ CartesianPositionControllerConfig();
+
+
+ // inherited from VariantDataClass
+ Ice::ObjectPtr ice_clone() const override
+ {
+ return this->clone();
+ }
+ VariantDataClassPtr clone(const Ice::Current& = ::Ice::Current()) const override
+ {
+ return new CartesianPositionControllerConfig(*this);
+ }
+ std::string output(const Ice::Current& c = ::Ice::Current()) const override;
+ VariantTypeId getType(const Ice::Current& = ::Ice::Current()) const override
+ {
+ return VariantType::CartesianPositionControllerConfig;
+ }
+ bool validate(const Ice::Current& = ::Ice::Current()) override
+ {
+ return true;
+ }
+
+ friend std::ostream& operator<<(std::ostream& stream, const CartesianPositionControllerConfig& rhs)
+ {
+ stream << "CartesianPositionControllerConfig: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ }
+
+ public: // serialization
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
+
+ };
+
+ typedef IceInternal::Handle<CartesianPositionControllerConfig> CartesianPositionControllerConfigPtr;
}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 3ec8849c620e0abbc05a548f625a771b76839199..0f84e0a0a2e54981497c4723009f297800e2bc02 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -119,6 +119,7 @@ Eigen::VectorXf CartesianVelocityController::calculateJointLimitAvoidance()
{
float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
r(i) = cos(f * M_PI);
+ //r(i) = math::MathUtils::Lerp(1.f, -1.f, f);
}
}
return r;
diff --git a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp
index e2d22fa0e321fc0ce6c3699466fc24b5002755c0..4305b29ceb6166b81d238e2309c799db83ce1e66 100644
--- a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp
+++ b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp
@@ -36,6 +36,7 @@
#include <RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h>
#include <RobotAPI/libraries/core/OrientedPoint.h>
#include <RobotAPI/libraries/core/FramedOrientedPoint.h>
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
using namespace armarx;
@@ -58,6 +59,7 @@ ObjectFactoryMap RobotAPIObjectFactories::getFactories()
add<armarx::OrientedPointBase, armarx::OrientedPoint>(map);
add<armarx::FramedOrientedPointBase, armarx::FramedOrientedPoint>(map);
+ add<armarx::CartesianPositionControllerConfigBase, armarx::CartesianPositionControllerConfig>(map);
add<armarx::PoseMedianFilterBase, armarx::filters::PoseMedianFilter>(map);
add<armarx::PoseAverageFilterBase, armarx::filters::PoseAverageFilter>(map);
diff --git a/source/RobotAPI/libraries/core/SimpleDiffIK.cpp b/source/RobotAPI/libraries/core/SimpleDiffIK.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aa25dffbb3d01bd4df15205e118ec2de1ab360e7
--- /dev/null
+++ b/source/RobotAPI/libraries/core/SimpleDiffIK.cpp
@@ -0,0 +1,85 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author SecondHands Demo (shdemo at armar6)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "CartesianPositionController.h"
+#include "CartesianVelocityController.h"
+#include "SimpleDiffIK.h"
+
+using namespace armarx;
+
+
+
+SimpleDiffIK::Result SimpleDiffIK::CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, Parameters params)
+{
+ tcp = tcp ? tcp : rns->getTCP();
+ CartesianVelocityController velocityController(rns);
+ CartesianPositionController positionController(tcp);
+
+ for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
+ {
+ Eigen::Vector3f posDiff = positionController.getPositionDiff(targetPose);
+ Eigen::Vector3f oriDiff = positionController.getOrientationDiff(targetPose);
+
+ //ARMARX_IMPORTANT << VAROUT(posDiff) << VAROUT(oriDiff);
+
+ Eigen::VectorXf cartesialVel(6);
+ cartesialVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
+ Eigen::VectorXf jnv = params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
+ Eigen::VectorXf jv = velocityController.calculate(cartesialVel, jnv, VirtualRobot::IKSolver::All);
+
+
+ float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
+ jv = jv * stepLength;
+
+ for (size_t n = 0; n < rns->getSize(); n++)
+ {
+ rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() + jv(n));
+ }
+ }
+
+ Result result;
+ result.jointValues = rns->getJointValuesEigen();
+ result.posDiff = positionController.getPositionDiff(targetPose);
+ result.oriDiff = positionController.getOrientationDiff(targetPose);
+ result.posError = positionController.getPositionError(targetPose);
+ result.oriError = positionController.getOrientationError(targetPose);
+ result.reached = result.posError < params.maxPosError && result.oriError < params.maxOriError;
+
+ result.jointLimitMargins = Eigen::VectorXf::Zero(rns->getSize());
+ result.minimumJointLimitMargin = FLT_MAX;
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ VirtualRobot::RobotNodePtr rn = rns->getNode(i);
+ if (rn->isLimitless())
+ {
+ result.jointLimitMargins(i) = M_PI;
+ }
+ else
+ {
+ result.jointLimitMargins(i) = std::min(rn->getJointValue() - rn->getJointLimitLo(), rn->getJointLimitHi() - rn->getJointValue());
+ result.minimumJointLimitMargin = std::min(result.minimumJointLimitMargin, result.jointLimitMargins(i));
+ }
+ }
+
+ return result;
+}
diff --git a/source/RobotAPI/libraries/core/SimpleDiffIK.h b/source/RobotAPI/libraries/core/SimpleDiffIK.h
new file mode 100644
index 0000000000000000000000000000000000000000..9f3302118b93230dd03cd402e04081314cc11ef5
--- /dev/null
+++ b/source/RobotAPI/libraries/core/SimpleDiffIK.h
@@ -0,0 +1,65 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
+namespace armarx
+{
+ typedef boost::shared_ptr<class SimpleDiffIK> SimpleDiffIKPtr;
+
+ class SimpleDiffIK
+ {
+ public:
+ struct Parameters
+ {
+ Parameters() {}
+ // IK params
+ float ikStepLengthInitial = 0.2f;
+ float ikStepLengthFineTune = 0.5f;
+ size_t stepsInitial = 25;
+ size_t stepsFineTune = 5;
+ float maxPosError = 10.f;
+ float maxOriError = 0.05f;
+ float jointLimitAvoidanceKp = 2.0f;
+ };
+ struct Result
+ {
+ Eigen::VectorXf jointValues;
+ Eigen::Vector3f posDiff;
+ Eigen::Vector3f oriDiff;
+ float posError;
+ float oriError;
+ bool reached;
+ Eigen::VectorXf jointLimitMargins;
+ float minimumJointLimitMargin;
+ };
+
+ static Result CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
+
+ };
+}
diff --git a/source/RobotAPI/libraries/core/SimpleGridReachability.cpp b/source/RobotAPI/libraries/core/SimpleGridReachability.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c13ea798af6433f002e02832de70ac8093834d4f
--- /dev/null
+++ b/source/RobotAPI/libraries/core/SimpleGridReachability.cpp
@@ -0,0 +1,44 @@
+#include "CartesianVelocityController.h"
+#include "SimpleGridReachability.h"
+
+using namespace armarx;
+
+
+SimpleGridReachability::Result SimpleGridReachability::calculateDiffIK(const Eigen::Matrix4f targetPose, const Parameters& params)
+{
+ VirtualRobot::RobotNodePtr rns = params.nodeSet;
+ VirtualRobot::RobotNodeSetPtr tcp = params.tcp ? params.tcp : rns->getTCP();
+ CartesianVelocityController velocityController(rns);
+ CartesianPositionController positionController(tcp);
+
+ Eigen::VectorXf initialJV = rns->getJointValuesEigen();
+ for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
+ {
+ Eigen::Vector3f posDiff = positionController.getPositionDiff(targetPose);
+ Eigen::Vector3f oriDiff = positionController.getOrientationDiff(targetPose);
+
+ //ARMARX_IMPORTANT << VAROUT(posDiff) << VAROUT(oriDiff);
+
+ Eigen::VectorXf cartesialVel(6);
+ cartesialVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
+ Eigen::VectorXf jnv = params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
+ Eigen::VectorXf jv = velocityController.calculate(cartesialVel, jnv, VirtualRobot::IKSolver::All);
+
+
+ float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
+ jv = jv * stepLength;
+
+ for (size_t n = 0; n < rns->getSize(); n++)
+ {
+ rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() + jv(n));
+ }
+ }
+
+ Result result;
+ result.jointValues = rns->getJointValuesEigen();
+ result.posError = positionController.getPositionDiff(targetPose);
+ result.oriError = positionController.getOrientationError(targetPose);
+ result.reached = result.posError < params.maxPosError && result.oriError < params.maxOriError;
+
+ return result;
+}
diff --git a/source/RobotAPI/libraries/core/SimpleGridReachability.h b/source/RobotAPI/libraries/core/SimpleGridReachability.h
new file mode 100644
index 0000000000000000000000000000000000000000..4ac9bc70f16524bbf86cbba53efc5f034a5c4f96
--- /dev/null
+++ b/source/RobotAPI/libraries/core/SimpleGridReachability.h
@@ -0,0 +1,37 @@
+#pragma once
+#include "CartesianPositionController.h"
+
+namespace armarx
+{
+ class SimpleGridReachability
+ {
+ public:
+ struct Parameters
+ {
+ VirtualRobot::RobotNodePtr tcp;
+ VirtualRobot::RobotNodeSetPtr nodeSet;
+
+ // IK params
+ float ikStepLengthInitial = 0.2f;
+ float ikStepLengthFineTune = 0.5f;
+ size_t stepsInitial = 25;
+ size_t stepsFineTune = 5;
+ float maxPosError = 10.f;
+ float maxOriError = 0.05f;
+ float jointLimitAvoidanceKp = 2.0f;
+ };
+ struct Result
+ {
+ Eigen::VectorXf jointValues;
+ float posError;
+ float oriError;
+ bool reached;
+ };
+
+ static Result CalculateDiffIK(const Eigen::Matrix4f targetPose, const Parameters& params);
+
+
+ };
+
+}
+