From fa0749eec4962d5fef630455dd1b022cca81f36e Mon Sep 17 00:00:00 2001
From: ArmarX User <armarx@kit.edu>
Date: Mon, 18 Feb 2019 14:15:06 +0100
Subject: [PATCH] changes for recording the films
---
.../DMPController/NJointTSDMPController.cpp | 22 +++++++++++++++++++
.../DMPController/NJointTSDMPController.h | 3 +++
.../NJointGripperZeroTorqueController.cpp | 10 ++++++---
.../NJointGripperZeroTorqueController.h | 1 +
4 files changed, 33 insertions(+), 3 deletions(-)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index a8fc99878..37dcd2291 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/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;
+ // }
+
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index 91fd0b4b6..498ec423e 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/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
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.cpp
index 4173f0942..0ce94824e 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.cpp
+++ b/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());
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.h
index f5c6accfc..2480895a5 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/GripperController/NJointGripperZeroTorqueController.h
@@ -47,6 +47,7 @@ namespace armarx
private:
std::vector<ControlTarget1DoFActuatorZeroTorque*> targets;
+ // std::vector<ControlTarget1DoFActuatorVelocity*> pumpTarget;
};
} // namespace armarx
--
GitLab