changes for recording the films

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp

@@ -98,7 +98,19 @@ namespace armarx
         filtered_position.setZero();
         pos_filter_factor = cfg->pos_filter;
 
+        //        jlhigh = rns->getNode("..")->getJointLimitHi();
+        //        jllow = rns->getNode("")->getJointLimitLo();
         firstRun = true;
+
+        jointLowLimits.setZero(targets.size());
+        jointHighLimits.setZero(targets.size());
+        for (size_t i = 0; i < rns->getSize(); i++)
+        {
+            VirtualRobot::RobotNodePtr rn = rns->getAllRobotNodes().at(i);
+
+            jointLowLimits(i) = rn->getJointLimitLo();
+            jointHighLimits(i) = rn->getJointLimitHi();
+        }
     }
 
     std::string NJointTSDMPController::getClassName(const Ice::Current&) const
@@ -184,9 +196,12 @@ namespace armarx
 
         Eigen::VectorXf qvel;
         qvel.resize(velocitySensors.size());
+        Eigen::VectorXf qpos;
+        qpos.resize(positionSensors.size());
         for (size_t i = 0; i < velocitySensors.size(); ++i)
         {
             qvel(i) = velocitySensors[i]->velocity;
+            qpos(i) = positionSensors[i]->position;
         }
 
         filtered_qvel = (1 - vel_filter_factor) * filtered_qvel + vel_filter_factor * qvel;
@@ -283,6 +298,7 @@ namespace armarx
         for (size_t i = 0; i < tcpController->rns->getSize(); i++)
         {
             jnv(i) += rtGetControlStruct().nullspaceJointVelocities.at(i);
+
             //            if (torqueSensors.at(i) && gravityTorqueSensors.at(i) && rtGetControlStruct().torqueKp.at(i) != 0)
             //            {
             //                torquePIDs.at(i).Kp = rtGetControlStruct().torqueKp.at(i);
@@ -308,6 +324,12 @@ namespace armarx
                                  << "Velocity controller target is invalid - setting to zero! set value: " << targets.at(i)->velocity;
                 targets.at(i)->velocity = 0.0f;
             }
+
+            //            if (qpos(i) > jointHighLimits(i) || qpos(i) < jointLowLimits(i))
+            //            {
+            //                targets.at(i)->velocity = 0;
+            //            }
+
         }
 
 

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h

@@ -173,6 +173,9 @@ namespace armarx
         float DpF;
         float KoF;
         float DoF;
+
+        Eigen::VectorXf jointHighLimits;
+        Eigen::VectorXf jointLowLimits;
     };
 
 } // namespace armarx

source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.cpp

@@ -17,6 +17,8 @@ namespace armarx
             ControlTargetBase* ct = useControlTarget(jointName, ControlModes::ZeroTorque1DoF);
             targets.push_back(ct->asA<ControlTarget1DoFActuatorZeroTorque>());
         };
+        //        ControlTargetBase* ctPump = useControlTarget("KIT_Gripper_Pump", ControlModes::Velocity1DoF);
+        //        pumpTarget.push_back(ctPump->asA<ControlTarget1DoFActuatorVelocity>());
     }
 
     std::string NJointGripperZeroTorqueController::getClassName(const Ice::Current&) const
@@ -25,14 +27,16 @@ namespace armarx
     }
 
     void NJointGripperZeroTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-    { 
+    {
         for (size_t i = 0; i < targets.size(); ++i)
         {
             targets.at(i)->torque = 0;
         }
+
+        //        pumpTarget.at(0)->velocity = 70.0;
     }
 
-    WidgetDescription::WidgetPtr NJointGripperZeroTorqueController::GenerateConfigDescription(const VirtualRobot::RobotPtr & robot, const std::map<std::string, ConstControlDevicePtr> & controlDevices, const std::map<std::string, ConstSensorDevicePtr> &)
+    WidgetDescription::WidgetPtr NJointGripperZeroTorqueController::GenerateConfigDescription(const VirtualRobot::RobotPtr& robot, const std::map<std::string, ConstControlDevicePtr>& controlDevices, const std::map<std::string, ConstSensorDevicePtr>&)
     {
         using namespace armarx::WidgetDescription;
         HBoxLayoutPtr layout = new HBoxLayout;
@@ -50,7 +54,7 @@ namespace armarx
         return layout;
     }
 
-    NJointGripperZeroTorqueControllerConfigPtr NJointGripperZeroTorqueController::GenerateConfigFromVariants(const StringVariantBaseMap &values)
+    NJointGripperZeroTorqueControllerConfigPtr NJointGripperZeroTorqueController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
     {
         return new NJointGripperZeroTorqueControllerConfig(values.at("nodeSetName")->getString());
     }

source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.h

@@ -47,6 +47,7 @@ namespace armarx
 
     private:
         std::vector<ControlTarget1DoFActuatorZeroTorque*> targets;
+        //        std::vector<ControlTarget1DoFActuatorVelocity*> pumpTarget;
     };
 
 } // namespace armarx