diff --git a/source/RobotAPI/CMakeLists.txt b/source/RobotAPI/CMakeLists.txt
index da048e516489529c8f6dcb8c6dcd7966fd8753cd..cdd33355000d13a62481122b001552634431cbba 100644
--- a/source/RobotAPI/CMakeLists.txt
+++ b/source/RobotAPI/CMakeLists.txt
@@ -3,3 +3,4 @@ add_subdirectory(motioncontrol)
add_subdirectory(applications)
add_subdirectory(units)
add_subdirectory(GraspingWithTorques)
+add_subdirectory(armarx-objects)
diff --git a/source/RobotAPI/core/RobotStatechartContext.h b/source/RobotAPI/core/RobotStatechartContext.h
index ac21018cf6c547c93aa51a83293062378d0e2eb6..47ba13f6e7f68509cf185176b1600ae491649023 100644
--- a/source/RobotAPI/core/RobotStatechartContext.h
+++ b/source/RobotAPI/core/RobotStatechartContext.h
@@ -22,8 +22,8 @@
*/
-#ifndef ARMARX_COMPONENT_Armar4Api_Armar4Context_H
-#define ARMARX_COMPONENT_Armar4Api_Armar4Context_H
+#ifndef ARMARX_COMPONENT_RobotApi_StatechartContext_H
+#define ARMARX_COMPONENT_RobotApi_StatechartContext_H
#include <Core/core/Component.h>
#include <Core/core/system/ImportExportComponent.h>
@@ -85,7 +85,6 @@ namespace armarx
KinematicUnitInterfacePrx kinematicUnitPrx;
KinematicUnitObserverInterfacePrx kinematicUnitObserverPrx;
TCPControlUnitInterfacePrx tcpControlPrx;
- //SystemObserverInterfacePrx systemObserver; // already defined in StatechartContext
VirtualRobot::RobotPtr remoteRobot;
private:
std::string kinematicUnitObserverName;
diff --git a/source/RobotAPI/motioncontrol/CMakeLists.txt b/source/RobotAPI/motioncontrol/CMakeLists.txt
index 5f0d70238320b66ebe080c1684f39daf4c039573..e004bdbdf77567900af350e185307ea4be2287fa 100644
--- a/source/RobotAPI/motioncontrol/CMakeLists.txt
+++ b/source/RobotAPI/motioncontrol/CMakeLists.txt
@@ -27,5 +27,8 @@ if (ARMARX_BUILD)
library_settings("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_HEADERS}")
target_link_ice(${LIB_NAME})
target_link_libraries(${LIB_NAME} ${LIBS})
+
+
endif()
+add_subdirectory(DMPControl)
diff --git a/source/RobotAPI/units/TCPControlUnit.cpp b/source/RobotAPI/units/TCPControlUnit.cpp
index 2b8cfb88ff45b9065a04d389d50ab1b0a5145a18..9cb31b2b2a52225fe14c182b572a7fd43a7dd037 100644
--- a/source/RobotAPI/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/units/TCPControlUnit.cpp
@@ -200,6 +200,7 @@ namespace armarx
ARMARX_IMPORTANT << "Requesting TCPControlUnit";
if(execTask)
execTask->stop();
+ lastReportTime = IceUtil::Time::now();
execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, true, "TCPVelocityCalculator");
execTask->start();
ARMARX_IMPORTANT << "Requested TCPControlUnit";
@@ -342,7 +343,8 @@ namespace armarx
EDifferentialIKPtr dIK = boost::shared_dynamic_cast<EDifferentialIK>(itDIK->second);
// ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << robotNodeSet->getTCP()->getGlobalPose();
// dIK->setVerbose(true);
- Eigen::VectorXf jointDelta = dIK->computeSteps(1, 0.00001, 100);
+ Eigen::VectorXf jointDelta;
+ dIK->computeSteps(jointDelta, 1, 0.00001, 100);
// ARMARX_VERBOSE << deactivateSpam(1) << "New Goal: \n" << robotNodeSet->getTCP()->getGlobalPose();
MatrixXf fullJac = dIK->calcFullJacobian();
@@ -403,13 +405,33 @@ namespace armarx
// localRobot->setJointValues(jointValues);
std::vector<RobotNodeSetPtr > nodeSets = localRobot->getRobotNodeSets();
FramedPoseBaseMap tcpPoses;
+ FramedVector3Map tcpTranslationVelocities;
+ FramedVector3Map tcpOrientationVelocities;
std::string rootFrame = localRobot->getRootNode()->getName();
+ double timeDelta = (IceUtil::Time::now() - lastReportTime).toSecondsDouble();
for(unsigned int i=0; i < nodeSets.size(); i++)
{
RobotNodeSetPtr nodeSet = nodeSets.at(i);
- tcpPoses[nodeSet->getTCP()->getName()]= new FramedPose(nodeSet->getTCP()->getGlobalPose(),rootFrame);
+ const std::string &tcpName = nodeSet->getTCP()->getName();
+ const Eigen::Matrix4f ¤tPose = nodeSet->getTCP()->getGlobalPose();
+ tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame);
+ FramedPoseBaseMap::iterator it = lastTCPPoses.find(tcpName);
+ if(it != lastTCPPoses.end())
+ {
+
+ FramedPosePtr lastPose = FramedPosePtr::dynamicCast(it->second);
+ tcpTranslationVelocities[tcpName] = new FramedVector3((currentPose.block(0,3,3,1) - lastPose->toEigen().block(0,3,3,1))/timeDelta, rootFrame);
+
+ const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
+ Eigen::AngleAxis<float> orient(rot.block<3,3>(0,0));
+ tcpOrientationVelocities[tcpName] = new FramedVector3(orient.axis() * (orient.angle()/timeDelta), rootFrame);
+ it->second = new FramedPose(currentPose, rootFrame);
+ }
}
listener->reportTCPPose(tcpPoses);
+ listener->reportTCPVelocities(tcpTranslationVelocities, tcpOrientationVelocities);
+ lastTCPPoses = tcpPoses;
+ lastReportTime = IceUtil::Time::now();
}
@@ -549,12 +571,18 @@ namespace armarx
this->dofWeights = dofWeights;
}
+ bool EDifferentialIK::computeSteps(float stepSize, float mininumChange, int maxNStep)
+ {
+ VectorXf jointDelta;
+ return computeSteps(jointDelta, stepSize, mininumChange, maxNStep, &EDifferentialIK::computeStep);
+ }
+
// void EDifferentialIK::setTCPWeights(Eigen::VectorXf tcpWeights)
// {
// this->tcpWeights = tcpWeights;
// }
- Eigen::VectorXf EDifferentialIK::computeSteps(float stepSize, float minumChange, int maxNStep)
+ bool EDifferentialIK::computeSteps(VectorXf &resultJointDelta, float stepSize, float mininumChange, int maxNStep, ComputeFunction computeFunction)
{
VR_ASSERT(rns);
VR_ASSERT(nodes.size() == rns->getSize());
@@ -571,8 +599,9 @@ namespace armarx
VectorXf oldJointValues;
rns->getJointValues(oldJointValues);
VectorXf tempDOFWeights = dofWeights;
- VectorXf dThetaSum(nodes.size());
- dThetaSum.setZero();
+// VectorXf dThetaSum(nodes.size());
+ resultJointDelta.resize(nodes.size());
+ resultJointDelta.setZero();
do
{
VectorXf dTheta = this->computeStep(stepSize);
@@ -588,7 +617,7 @@ namespace armarx
{
VR_WARNING << "Aborting, invalid joint value (nan)" << endl;
dofWeights = tempDOFWeights;
- return dThetaSum;
+ return false;
}
//nodes[i]->setJointValue(nodes[i]->getJointValue() + dTheta[i]);
}
@@ -597,7 +626,7 @@ namespace armarx
VectorXf newJointValues;
rns->getJointValues(newJointValues);
- dThetaSum = newJointValues - oldJointValues;
+ resultJointDelta = newJointValues - oldJointValues;
// ARMARX_INFO << "joint angle deltas:\n" << dThetaSum;
// check tolerances
@@ -608,7 +637,7 @@ namespace armarx
break;
}
float d = dTheta.norm();
- if (dTheta.norm()<minumChange)
+ if (dTheta.norm()<mininumChange)
{
if (verbose)
VR_INFO << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << endl;
@@ -626,7 +655,8 @@ namespace armarx
jvBest = jv;
lastDist = posDist;
step++;
- jointDeltaIterations.push_back(std::make_pair(getWeightedError(), dThetaSum));
+
+ jointDeltaIterations.push_back(std::make_pair(getWeightedError(), resultJointDelta));
}
while (step<maxNStep);
@@ -637,24 +667,28 @@ namespace armarx
if(jointDeltaIterations.at(i).first < bestJVError)
{
bestJVError = jointDeltaIterations.at(i).first;
- dThetaSum = jointDeltaIterations.at(i).second;
+ resultJointDelta = jointDeltaIterations.at(i).second;
bestIndex = i;
}
}
+ robot->setJointValues(rns,oldJointValues + resultJointDelta);
+
// ARMARX_INFO << "best try: " << bestIndex << " with error: " << bestJVError;
// rns->setJointValues(oldJointValues);
// Matrix4f oldPose = rns->getTCP()->getGlobalPose();
// rns->setJointValues(oldJointValues+dThetaSum);
// Matrix4f newPose = rns->getTCP()->getGlobalPose();
// ARMARX_IMPORTANT << "tcp delta:\n" << newPose.block(0,3,3,1) - oldPose.block(0,3,3,1);
+ dofWeights = tempDOFWeights;
if (step >= maxNStep && verbose)
{
VR_INFO << "IK failed, loop:" << step << endl;
VR_INFO << "pos error:" << getErrorPosition() << endl;
VR_INFO << "rot error:" << getErrorRotation() << endl;
+ return false;
}
- dofWeights = tempDOFWeights;
- return dThetaSum;
+
+ return true;
}
VectorXf EDifferentialIK::computeStep(float stepSize )
@@ -707,7 +741,7 @@ namespace armarx
}
- applyWeightsToJacobian(Jacobian);
+ applyDOFWeightsToJacobian(Jacobian);
MatrixXf pseudo = computePseudoInverseJacobianMatrix(Jacobian);
@@ -721,6 +755,8 @@ namespace armarx
if (nRows==0) this->setNRows();
size_t nDoF = nodes.size();
+ std::map<float, MatrixXf> partJacobians;
+
VectorXf thetaSum(nDoF);
thetaSum.setZero();
size_t index=0;
@@ -775,7 +811,7 @@ namespace armarx
ARMARX_INFO << deactivateSpam(0.05) << "step size adjusted error to goal:\n" << partError;
- applyWeightsToJacobian(partJacobian);
+ applyDOFWeightsToJacobian(partJacobian);
// ARMARX_INFO << "Jac:\n" << partJacobian;
@@ -836,7 +872,24 @@ namespace armarx
return thetaSum;
}
- void EDifferentialIK::applyWeightsToJacobian(MatrixXf &Jacobian)
+ bool EDifferentialIK::solveIK(float stepSize, float minChange, int maxSteps, bool useAlternativeOnFail)
+ {
+ Eigen::VectorXf jointValuesBefore;
+ rns->getJointValues(jointValuesBefore);
+ Eigen::VectorXf resultJointDelta;
+ Eigen::VectorXf jointDeltaAlternative;
+ bool result = computeSteps(resultJointDelta, stepSize,minChange, maxSteps);
+ float error = getWeightedError();
+ if(useAlternativeOnFail && error > 5)
+ result = computeSteps(jointDeltaAlternative,stepSize,minChange, maxSteps, &EDifferentialIK::computeStepIndependently);
+ if(getWeightedError() < error)
+ {
+ resultJointDelta = jointDeltaAlternative;
+ }
+ return result;
+ }
+
+ void EDifferentialIK::applyDOFWeightsToJacobian(MatrixXf &Jacobian)
{
if(dofWeights.rows() == Jacobian.cols())
{
diff --git a/source/RobotAPI/units/TCPControlUnit.h b/source/RobotAPI/units/TCPControlUnit.h
index 4bd9ff51d159a16dcb2ffec3e4fea9dacca85024..fde9222c017e75624e29f1968e3f8a22001a58da 100644
--- a/source/RobotAPI/units/TCPControlUnit.h
+++ b/source/RobotAPI/units/TCPControlUnit.h
@@ -122,6 +122,10 @@ namespace armarx {
bool requested;
+
+ FramedPoseBaseMap lastTCPPoses;
+ IceUtil::Time lastReportTime;
+
Mutex dataMutex;
Mutex taskMutex;
@@ -130,6 +134,8 @@ namespace armarx {
class EDifferentialIK : public VirtualRobot::DifferentialIK, virtual public Logging
{
public:
+ typedef Eigen::VectorXf (EDifferentialIK::*ComputeFunction)(float);
+
EDifferentialIK(VirtualRobot::RobotNodeSetPtr rns,VirtualRobot:: RobotNodePtr coordSystem = VirtualRobot::RobotNodePtr(), VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = eSVD);
VirtualRobot::RobotNodePtr getRefFrame(){return coordSystem;}
@@ -144,14 +150,16 @@ namespace armarx {
void setDOFWeights(Eigen::VectorXf dofWeights);
// void setTCPWeights(Eigen::VectorXf tcpWeights);
- Eigen::VectorXf computeSteps(float stepSize, float minumChange, int maxNStep);
+ bool computeSteps(float stepSize = 1.f, float mininumChange = 0.01f, int maxNStep = 50);
+ bool computeSteps(Eigen::VectorXf& resultJointDelta, float stepSize = 1.f, float mininumChange = 0.01f, int maxNStep = 50, ComputeFunction computeFunction = NULL);
Eigen::VectorXf computeStep(float stepSize);
- void applyWeightsToJacobian(Eigen::MatrixXf &Jacobian);
+ void applyDOFWeightsToJacobian(Eigen::MatrixXf &Jacobian);
void applyTCPWeights(VirtualRobot::RobotNodePtr tcp, Eigen::MatrixXf &partJacobian);
void applyTCPWeights(Eigen::MatrixXf &invJacobian);
float getWeightedError();
float getWeightedErrorPosition(VirtualRobot::RobotNodePtr tcp);
Eigen::VectorXf computeStepIndependently(float stepSize);
+ bool solveIK(float stepSize = 1, float minChange = 0.01f, int maxSteps = 50, bool useAlternativeOnFail = false);
protected:
bool adjustDOFWeightsToJointLimits(const Eigen::VectorXf &plannedJointDeltas);
diff --git a/source/RobotAPI/units/TCPControlUnitObserver.cpp b/source/RobotAPI/units/TCPControlUnitObserver.cpp
index 20ae2d59ecdfecab25d2a9681d052aa732ec8e75..28644c10e89c0e10acaf7b43110317859df9dec7 100644
--- a/source/RobotAPI/units/TCPControlUnitObserver.cpp
+++ b/source/RobotAPI/units/TCPControlUnitObserver.cpp
@@ -30,6 +30,10 @@
#include <Core/observers/checks/ConditionCheckLarger.h>
#include <Core/observers/checks/ConditionCheckSmaller.h>
#include <Core/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h>
+#include <Core/core/exceptions/local/ExpressionException.h>
+
+#define TCP_POSE_CHANNEL "TCPPose"
+#define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
namespace armarx {
@@ -51,8 +55,8 @@ namespace armarx {
// offerConditionCheck("equalsPoseWithTol", new ConditionCheckEqualsPoseWithTolerance());
- offerChannel("TCPPose", "TCP poses of the robot.");
- offerChannel("TCPVelocities", "TCP velocities of the robot.");
+ offerChannel(TCP_POSE_CHANNEL, "TCP poses of the robot.");
+ offerChannel(TCP_TRANS_VELOCITIES_CHANNEL, "TCP velocities of the robot.");
}
@@ -76,10 +80,10 @@ namespace armarx {
FramedPosePtr vec = FramedPosePtr::dynamicCast(it->second);
const std::string &tcpName = it->first;
- if(!existsDataField("TCPPose", tcpName))
- offerDataFieldWithDefault("TCPPose", tcpName, Variant(it->second), "Pose of " + tcpName);
+ if(!existsDataField(TCP_POSE_CHANNEL, tcpName))
+ offerDataFieldWithDefault(TCP_POSE_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
else
- setDataField("TCPPose", tcpName, Variant(it->second));
+ setDataField(TCP_POSE_CHANNEL, tcpName, Variant(it->second));
if(!existsChannel(tcpName))
{
@@ -108,8 +112,48 @@ namespace armarx {
}
}
- void TCPControlUnitObserver::reportTCPVelocities(const FramedVector3Map &, const FramedVector3Map &, const Ice::Current &)
+ void TCPControlUnitObserver::reportTCPVelocities(const FramedVector3Map &tcpTranslationVelocities, const FramedVector3Map &tcpOrientationVelocities, const Ice::Current &)
{
+ ScopedLock lock(dataMutex);
+ FramedVector3Map::const_iterator it = tcpTranslationVelocities.begin();
+ for(; it != tcpTranslationVelocities.end(); it++)
+ {
+
+ FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(it->second);
+ FramedVector3Ptr vecOri;
+ FramedVector3Map::const_iterator itOri = tcpOrientationVelocities.find(it->first);
+ if(itOri != tcpOrientationVelocities.end())
+ vecOri = FramedVector3Ptr::dynamicCast(itOri->second);
+ const std::string &tcpName = it->first;
+ ARMARX_CHECK_EXPRESSION(vec->frame == vecOri->frame);
+
+ if(!existsDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName))
+ offerDataFieldWithDefault(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
+ else
+ setDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second));
+ if(!existsChannel(tcpName))
+ {
+ offerChannel(tcpName, "pose components of " + tcpName);
+ offerDataFieldWithDefault(tcpName,"x", Variant(vec->x), "X axis");
+ offerDataFieldWithDefault(tcpName,"y", Variant(vec->y), "Y axis");
+ offerDataFieldWithDefault(tcpName,"z", Variant(vec->z), "Z axis");
+ offerDataFieldWithDefault(tcpName,"roll", Variant(vecOri->x), "Roll");
+ offerDataFieldWithDefault(tcpName,"pitch", Variant(vecOri->y), "Pitch");
+ offerDataFieldWithDefault(tcpName,"yaw", Variant(vecOri->z), "Yaw");
+ offerDataFieldWithDefault(tcpName,"frame", Variant(vecOri->frame), "Reference Frame");
+ }
+ else
+ {
+ setDataField(tcpName,"x", Variant(vec->x));
+ setDataField(tcpName,"y", Variant(vec->y));
+ setDataField(tcpName,"z", Variant(vec->z));
+ setDataField(tcpName,"roll", Variant(vecOri->x));
+ setDataField(tcpName,"pitch", Variant(vecOri->y));
+ setDataField(tcpName,"yaw", Variant(vecOri->z));
+ setDataField(tcpName,"frame", Variant(vec->frame));
+ }
+
+ }
}
}
diff --git a/source/RobotAPI/units/TCPControlUnitObserver.h b/source/RobotAPI/units/TCPControlUnitObserver.h
index 727a9a17ac9300ad0afc94865ee4b952a58aa24a..20f00a1cc0df69a7ace08ce6b998548943b8fe68 100644
--- a/source/RobotAPI/units/TCPControlUnitObserver.h
+++ b/source/RobotAPI/units/TCPControlUnitObserver.h
@@ -58,8 +58,8 @@ namespace armarx {
// TCPControlUnitListener interface
public:
- void reportTCPPose(const FramedPoseBaseMap &poseMap, const Ice::Current &);
- void reportTCPVelocities(const FramedVector3Map &, const FramedVector3Map &, const Ice::Current &);
+ void reportTCPPose(const FramedPoseBaseMap &poseMap, const Ice::Current &c = Ice::Current());
+ void reportTCPVelocities(const FramedVector3Map &tcpTranslationVelocities, const FramedVector3Map &tcpOrientationVelocities, const Ice::Current &c = Ice::Current());
Mutex dataMutex;
};