From 0fb6db9cc8c9438760a2ea050527086312b1f6d7 Mon Sep 17 00:00:00 2001
From: Markus <Markus.Przybylski@kit.edu>
Date: Tue, 18 Feb 2014 17:00:34 +0100
Subject: [PATCH] Version of GraspingWithTorques where joint torques are
interpreted as joint velocities... (because setting torques directly to the
joints does not work in Bullet so far). Joints move, but motion will not stop
(no self collision detection).
---
.../GraspingWithTorques.cpp | 95 ++++++++++++-------
1 file changed, 63 insertions(+), 32 deletions(-)
diff --git a/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp b/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp
index fe01e4fd8..37b4156fa 100644
--- a/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp
+++ b/source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp
@@ -24,9 +24,7 @@
#include "GraspingWithTorques.h"
//#include "Armar3GraspContext.h" //MP: Maybe necessary again later?
-#include "../core/RobotStatechartContext.h" //MP 2014-01-21
-
-//#include "../../units/KinematicUnitHand/KinematicUnitHand.h"
+#include "../core/RobotStatechartContext.h"
#include <Core/observers/variant/ChannelRef.h>
#include <Core/observers/variant/SingleTypeVariantList.h>
@@ -61,13 +59,13 @@ namespace armarx
addToInput("thresholdVelocity", VariantType::Float, false);
- addToInput("robotNodeSetName", VariantType::String, false); //RobotNodeSet: LeftHand (auskommentarisiert MP 2014-01-21)
+ addToInput("robotNodeSetName", VariantType::String, false);
addToLocal("jointNames", VariantType::List(VariantType::String));
- //addToLocal("jointNames", SingleTypeVariantList(VariantType::String)); //Geht das? (2014-01-21)
- addToLocal("jointVelocityChannel", VariantType::ChannelRef);
- //addToLocal("jointVelocityChannel", VariantType::ChannelRef, false);
+ //addToLocal("jointVelocityChannel", VariantType::ChannelRef); //first try ...
+
+ addToLocal("jointVelocitiesDatafields", VariantType::List(VariantType::DatafieldRef));
}
@@ -93,7 +91,8 @@ namespace armarx
->mapFromParent("timeoutGrasp", "timeoutGrasp")
->mapFromParent("thresholdVelocity", "thresholdVelocity")
->mapFromParent("jointNames")
- ->mapFromParent("jointVelocityChannel"); //bei gleichem Namen muss man ihn nur einmal angeben
+ ->mapFromParent("jointVelocitiesDatafields");
+ //->mapFromParent("jointVelocityChannel"); //first try...
setInitState(statePreshape, mapPreshapeInfo);
@@ -110,18 +109,24 @@ namespace armarx
ARMARX_LOG << eINFO << "Entering StatechartGraspingWithTorques";
RobotStatechartContext* context = getContext<RobotStatechartContext>();
- setLocal("jointVelocityChannel", context->getChannelRef(context->getKinematicUnitObserverName(),"jointvelocities"));
+ //setLocal("jointVelocityChannel", context->getChannelRef(context->getKinematicUnitObserverName(),"jointvelocities"));
+ ChannelRefPtr tempChannelRef = context->getChannelRef(context->getKinematicUnitObserverName(),"jointvelocities");
VirtualRobot::RobotNodeSetPtr nodeSet = context->remoteRobot->getRobotNodeSet(getInput<std::string>("robotNodeSetName")); //woher robotNodeSetName holen? (Argument für getRobotNodeSet())
SingleTypeVariantList jointNames(VariantType::String);
+ SingleTypeVariantList dataFields(VariantType::DatafieldRef);
+
for (int i=0; i<nodeSet->getSize(); i++)
{
jointNames.addVariant(nodeSet->getNode(i)->getName()); //nodes = joints
+ dataFields.addVariant(context->getDatafieldRef(tempChannelRef, nodeSet->getNode(i)->getName()));
}
- setLocal("jointNames",jointNames); //befüllen...!?
+ setLocal("jointNames", jointNames); //befüllen...!?
+ setLocal("jointVelocitiesDatafields", dataFields);
+
}
void StatechartGraspingWithTorques::onExit()
@@ -140,8 +145,8 @@ namespace armarx
addToInput("jointAnglesPreshape", VariantType::List(VariantType::Float), false);
addToInput("timeoutPreshape", VariantType::Float, false);
- //addToLocal("jointNames", VariantType::List(VariantType::String)); //ACHTUNG: addToLocal() oder addToInput()? (2013-01-21)
- addToInput("jointNames", VariantType::List(VariantType::String), false); //ACHTUNG: addToLocal() oder addToInput()? (2013-01-21)
+ //addToLocal("jointNames", VariantType::List(VariantType::String));
+ addToInput("jointNames", VariantType::List(VariantType::String), false);
}
void StatePreshape::onEnter()
@@ -151,7 +156,6 @@ namespace armarx
RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
SingleTypeVariantListPtr jointAnglesPreshapeList = getInput<SingleTypeVariantList>("jointAnglesPreshape");
- //NameValueMap jointAnglesPreshapeMap;
NameValueMap jointNamesAndValues;
if (jointNames->getSize() == jointAnglesPreshapeList->getSize())
@@ -164,9 +168,6 @@ namespace armarx
else
throw LocalException("StatePreshape: List lengths do not match!");
- //ARMARX_INFO << "node.size()=" << nodes.size() << flush;
- //ARMARX_INFO << "jointAnglesPreshapeList.size()=" << jointAnglesPreshapeList->getSize() << flush;
-
rsContext->kinematicUnitPrx->setJointAngles(jointNamesAndValues);
ARMARX_LOG << eINFO << "Installing preshape timeout condition";
@@ -241,8 +242,10 @@ namespace armarx
addToInput("jointNames", VariantType::List(VariantType::String), false);
- //addToLocal("jointVelocityChannel", VariantType::ChannelRef);
- addToInput("jointVelocityChannel", VariantType::ChannelRef, false);
+ //addToInput("jointVelocityChannel", VariantType::ChannelRef, false); //first try ...
+
+ addToInput("jointVelocitiesDatafields", VariantType::List(VariantType::DatafieldRef), false);
+
}
void StateInstallTerminateConditions::onEnter()
@@ -256,20 +259,11 @@ namespace armarx
ARMARX_LOG << eINFO << "Installing allJointVelocitiesLow condition";
float thresholdVelocity = getInput<float>("thresholdVelocity");
- /*
- Literal JointVelocityLowJoint0(getInput<ChannelRef>("jointVelocityChannel")
- ->getDataFieldIdentifier(getVariant->getString()), "smaller", Literal::createParameterList(thresholdVelocity)); //siehe SingleTypeVariantList, Auslesen mit getInput()
- Literal JointVelocityLowJoint1(getInput<ChannelRef>(markerChannelName)
- ->getDataFieldIdentifier("localized"), "smaller", Literal::createParameterList(thresholdVelocity));
- */
-
- //Term allJointVelocitiesLow = JointVelocityLowJoint0 && JointVelocityLowJoint1; //TODO: weitere Literale
- //condAllJointVelocitiesLow = installCondition(allJointVelocitiesLow, createEvent<EvAllJointVelocitiesLow>());
-
- //TODO: korrekter ChannelName für Hand-/Fingergelenke? Korrekter DatafieldIdentifier?
- //TODO: Condition zusammenbauen in einer for-Schleife (siehe ArmarX-Doku zu Conditions...)
- //TODO: korrekte Parametrierung von getDataFieldIdentifier(), mit getInput()...
+ //=======
+ //first try ... (with ChannelRef)
+ //=======
+ /*
Term allJointVelocitiesLow;
SingleTypeVariantListPtr jointNamesList = getInput<SingleTypeVariantList>("jointNames");
for (int i=0; i<jointNamesList->getSize(); i++)
@@ -277,8 +271,25 @@ namespace armarx
allJointVelocitiesLow = allJointVelocitiesLow && Literal(getInput<ChannelRef>("jointVelocityChannel")
->getDataFieldIdentifier(jointNamesList->getVariant(i)->getString()), "smaller", Literal::createParameterList(thresholdVelocity));
}
-
condAllJointVelocitiesLow = installCondition(allJointVelocitiesLow, createEvent<EvAllJointVelocitiesLow>());
+ */
+
+ //=======
+ //second try ... (with DatafieldRef)
+ //=======
+ Term allJointVelocitiesLow_NEW;
+ //SingleTypeVariantListPtr jointNamesList = getInput<SingleTypeVariantList>("jointNames");
+ SingleTypeVariantListPtr dataFieldsList = getInput<SingleTypeVariantList>("jointVelocitiesDatafields");
+
+ //for (int i=0; i<jointNamesList->getSize(); i++)
+ for (int i=0; i<dataFieldsList->getSize(); i++)
+ {
+ allJointVelocitiesLow_NEW = allJointVelocitiesLow_NEW &&
+ Literal(dataFieldsList->getVariant(i)->get<DatafieldRef>()->getDataFieldIdentifier(),
+ "inrange", Literal::createParameterList(-thresholdVelocity, thresholdVelocity));
+ }
+
+ condAllJointVelocitiesLow = installCondition(allJointVelocitiesLow_NEW, createEvent<EvAllJointVelocitiesLow>());
}
@@ -288,6 +299,26 @@ namespace armarx
removeTimeoutEvent(condGraspTimeout);
removeCondition(condAllJointVelocitiesLow);
+ //---------
+ //set joint velocities manually to zero (DEBUG)
+ //---------
+
+ RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
+ SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
+ //SingleTypeVariantListPtr jointTorquesGraspList = getInput<SingleTypeVariantList>("jointTorquesGrasp");
+ NameValueMap jointNamesAndValues;
+
+ for (int j=0; j<jointNames->getSize(); j++)
+ {
+ jointNamesAndValues[jointNames->getVariant(j)->getString()]=0.0f; //set everything to zero
+ }
+
+
+ //rsContext->kinematicUnitPrx->setJointTorques(jointNamesAndValues);
+ rsContext->kinematicUnitPrx->setJointVelocities(jointNamesAndValues);
+
+
+
}
}
--
GitLab