Velocity Control Statechart added in MotionControl

git-svn-id: https://svn.sfb588.uni-karlsruhe.de/svn/HumanoidRobotAPI@4 e7ad446d-f7d8-486a-9efb-df2f5bbea5c6

motioncontrol/MotionControl.cpp

@@ -129,6 +129,99 @@ void MoveJoints::onExit()
     removeTimeoutEvent(timeoutEvent);
 }
 
+void MoveJointsVelocityControl::defineParameters()
+{
+
+    context = getContext<RobotStatechartContext>();
+
+    std::string configFilepathAbsolute;
+    ArmarXDataPath::getAbsolutePath("HumanoidRobotAPIConfigs/stateconfigs/MotionControl.xml", configFilepathAbsolute);
+    setConfigFile(configFilepathAbsolute);
+
+    addListToInput("jointNames", VariantType::String, false);
+    addListToInput("targetJointVelocities", VariantType::Float, false);
+
+    // TODO: add when we have decided how to do it
+    addListToInput("jointMaxSpeeds", VariantType::Float, true);
+    addToInput("jointMaxSpeed", VariantType::Float, true);
+
+    addToInput("accelerationTime", VariantType::Float, false);
+
+    addToInput("targetJointVelocityTolerance", VariantType::Float, false);
+
+    addToInput("timeoutInMs", VariantType::Int, false);
+
+    addListToOutput("jointVelocitiesDistancesToTargets", VariantType::Float, true);
+
+
+}
+
+void MoveJointsVelocityControl::onEnter()
+{
+
+    float accelerationTime = getInput<float>("accelerationTime");
+    NameValueMap targetJointVelocities;
+    SingleTypeVariantListPtr jointNames = getInputList("jointNames");
+    SingleTypeVariantListPtr targetVelocitiesValues = getInputList("targetJointVelocities");
+
+    if (jointNames->getSize()!=targetVelocitiesValues->getSize())
+    {
+        throw LocalException("Sizes of joint name list and joint velocities list do not match!");
+    }
+
+    for(int i=0; i<jointNames->getSize(); i++ )
+    {
+        targetJointVelocities[jointNames->getElement(i)->getString()] = targetVelocitiesValues->getElement(i)->getFloat();
+        ARMARX_VERBOSE << "setting joint angle for joint '" << jointNames->getElement(i)->getString() << "' to " << targetVelocitiesValues->getElement(i)->getFloat() << std::endl;
+    }
+    // TODO: Set Max Velocities
+
+    // now install the condition
+
+    ConditionPtr cond = Condition::create();
+    float tolerance = getInput<float>("targetJointVelocityTolerance");
+    for(int i=0; i<jointNames->getSize(); i++ )
+    {
+        ParameterList inrangeCheck;
+        inrangeCheck.push_back(new Variant(targetVelocitiesValues->getElement(i)->getFloat()-tolerance));
+        inrangeCheck.push_back(new Variant(targetVelocitiesValues->getElement(i)->getFloat()+tolerance));
+
+        cond->add(DataFieldIdentifier(context->getKinematicUnitObserverName(),"jointvelocities", jointNames->getElement(i)->getString()), "inrange", inrangeCheck);
+    }
+
+    targetVelocitiesReachedCondition = installCondition<EvJointVelocitiesTargetReached>(cond);
+    ARMARX_INFO << "condition installed" << std::endl;
+    timeoutEvent = setTimeoutEvent(getInput<int>("timeoutInMs"), createEvent<EvTimeout>());
+    ARMARX_INFO << "timeout installed" << std::endl;
+//    context->kinematicUnitPrx->setAccelerationTime(accelerationTime);
+    context->kinematicUnitPrx->setJointVelocities(targetJointVelocities);
+}
+
+void MoveJointsVelocityControl::onExit()
+{
+
+    SingleTypeVariantListPtr jointNames = getInputList("jointNames");
+    SingleTypeVariantListPtr targetJointVelocities = getInputList("targetJointVelocities");
+    SingleTypeVariantList resultList(VariantType::Float);
+
+
+
+    for(int i=0; i<jointNames->getSize(); i++ )
+    {
+        DataFieldIdentifierPtr datafield = new DataFieldIdentifier(context->getKinematicUnitObserverName(),"jointvelocities", jointNames->getElement(i)->getString());
+        float jointVelocityActual = context->kinematicUnitObserverPrx->getDataField(datafield)->getFloat();
+        float jointVelocityTarget = targetJointVelocities->getElement(i)->getFloat();
+        resultList.addElement(new Variant(jointVelocityActual - jointVelocityTarget));
+    }
+    setOutput("jointVelocitiesDistancesToTargets", resultList);
+
+    ARMARX_INFO << "output: " << StateUtilFunctions::getDictionaryString(getOutputParameters()) << std::endl;
+
+    removeCondition(targetVelocitiesReachedCondition);
+    removeTimeoutEvent(timeoutEvent);
+
+}
+
 void MoveTCPPoseIK::defineParameters()
 {
 

motioncontrol/MotionControl.h

@@ -19,6 +19,7 @@ namespace MoveHelpers
     };
 
     DEFINEEVENT(EvJointTargetReached)
+    DEFINEEVENT(EvJointVelocitiesTargetReached)
     DEFINEEVENT(EvCalculationDone)
     DEFINEEVENT(EvTimeout)
 
@@ -40,7 +41,15 @@ namespace MoveHelpers
         void onExit();
     };
 
-
+    struct MoveJointsVelocityControl : StateTemplate<MoveJointsVelocityControl>
+        {
+            RobotStatechartContext* context;
+            ConditionIdentifier targetVelocitiesReachedCondition;
+            ActionEventIdentifier timeoutEvent;
+            void defineParameters();
+            void onEnter();
+            void onExit();
+        };
 
     struct MoveTCPPoseIK : StateTemplate<MoveTCPPoseIK>
     {