diff --git a/source/armarx/kinesthetic_learning/components/kinesthetic_learning_memory/Component.h b/source/armarx/kinesthetic_learning/components/kinesthetic_learning_memory/Component.h
index 4986f219dc22d6f976d7579002f9aae437fbebc6..f74978e432ed6d5a71f02861eb10113d003ae468 100644
--- a/source/armarx/kinesthetic_learning/components/kinesthetic_learning_memory/Component.h
+++ b/source/armarx/kinesthetic_learning/components/kinesthetic_learning_memory/Component.h
@@ -71,7 +71,6 @@ namespace armarx::kinesthetic_learning::components::kinesthetic_learning_memory
void onConnectComponent() override;
private:
-
struct Properties
{
};
diff --git a/source/armarx/kinesthetic_learning/components/kinesthetic_teaching/Component.cpp b/source/armarx/kinesthetic_learning/components/kinesthetic_teaching/Component.cpp
index 6d93dcabda454240977ce46ad82c4d6f9e523381..6626cee156e24b49d6003b7624edbfccb16f3114 100644
--- a/source/armarx/kinesthetic_learning/components/kinesthetic_teaching/Component.cpp
+++ b/source/armarx/kinesthetic_learning/components/kinesthetic_teaching/Component.cpp
@@ -41,7 +41,6 @@
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
-#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <armarx/kinesthetic_learning/kinesthetic_teaching/BimanualKinestheticTeaching.h>
#include <armarx/kinesthetic_learning/kinesthetic_teaching/FTWristSensor.h>
@@ -159,7 +158,8 @@ namespace armarx::kinesthetic_learning::components::kinesthetic_teaching
ARMARX_CHECK_NOT_NULL(getRobotUnit());
ARMARX_CHECK_NOT_NULL(robot);
- const armarx::armem::robot_state::description::RobotDescription robotDescription{.name = robot->getName()};
+ const armarx::armem::robot_state::description::RobotDescription robotDescription{
+ .name = robot->getName()};
motionRecorder.reset();
motionRecorder.emplace(robotDescription, virtualRobotReader_->get());
@@ -188,8 +188,11 @@ namespace armarx::kinesthetic_learning::components::kinesthetic_teaching
SingleArmKinestheticTeaching rightArmKinTeaching(
rightTrigger, rightArmZeroTorqueController, properties.robot.rns.rightArm);
- BimanualKinestheticTeaching kinestheticTeaching(
- leftArmKinTeaching, rightArmKinTeaching, *motionRecorder, sceneRecorder, properties.bimanual);
+ BimanualKinestheticTeaching kinestheticTeaching(leftArmKinTeaching,
+ rightArmKinTeaching,
+ *motionRecorder,
+ sceneRecorder,
+ properties.bimanual);
ARMARX_IMPORTANT << "You can now use the FT sensor in the wrist to start the recording.";
kinestheticTeaching.run();
@@ -210,7 +213,8 @@ namespace armarx::kinesthetic_learning::components::kinesthetic_teaching
ARMARX_INFO << "Running in simulation";
- const armarx::armem::robot_state::description::RobotDescription robotDescription{.name = robot->getName()};
+ const armarx::armem::robot_state::description::RobotDescription robotDescription{
+ .name = robot->getName()};
motionRecorder.reset();
motionRecorder.emplace(robotDescription, virtualRobotReader_->get());
@@ -228,8 +232,11 @@ namespace armarx::kinesthetic_learning::components::kinesthetic_teaching
SingleArmKinestheticTeaching rightArmKinTeaching(
rightTrigger, rightArmZeroTorqueController, properties.robot.rns.rightArm);
- BimanualKinestheticTeaching kinestheticTeaching(
- leftArmKinTeaching, rightArmKinTeaching, *motionRecorder, sceneRecorder, properties.bimanual);
+ BimanualKinestheticTeaching kinestheticTeaching(leftArmKinTeaching,
+ rightArmKinTeaching,
+ *motionRecorder,
+ sceneRecorder,
+ properties.bimanual);
ARMARX_IMPORTANT << "Running in simulation. Will be automated.";
kinestheticTeaching.run();
@@ -307,8 +314,9 @@ namespace armarx::kinesthetic_learning::components::kinesthetic_teaching
ARMARX_INFO << "Connected.";
{
- const auto providerId =
- armem::MemoryID(memory::constants::MemoryName, memory::constants::DemonstrationCoreSegment, getName());
+ const auto providerId = armem::MemoryID(memory::constants::MemoryName,
+ memory::constants::DemonstrationCoreSegment,
+ getName());
const auto timestamp = Clock::Now();
diff --git a/source/armarx/kinesthetic_learning/components/kinesthetic_teaching/Component.h b/source/armarx/kinesthetic_learning/components/kinesthetic_teaching/Component.h
index bfcad3157bff6ea42a8efa44996e655f6eeb76f3..806e3cf843d444f0503641ddb40cd1b75237d019 100644
--- a/source/armarx/kinesthetic_learning/components/kinesthetic_teaching/Component.h
+++ b/source/armarx/kinesthetic_learning/components/kinesthetic_teaching/Component.h
@@ -24,7 +24,6 @@
#pragma once
-#include <armarx/kinesthetic_learning/kinesthetic_teaching/BimanualKinestheticTeaching.h>
#include <experimental/memory>
#include <string>
@@ -45,6 +44,7 @@
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
#include <armarx/kinesthetic_learning/components/kinesthetic_teaching/ComponentInterface.h>
+#include <armarx/kinesthetic_learning/kinesthetic_teaching/BimanualKinestheticTeaching.h>
#include <armarx/kinesthetic_learning/kinesthetic_teaching/MotionRecorder.h>
#include <armarx/kinesthetic_learning/kinesthetic_teaching/SceneRecorder.h>
@@ -141,8 +141,8 @@ namespace armarx::kinesthetic_learning::components::kinesthetic_teaching
// armarx::plugins::RobotUnitComponentPlugin* robotUnitPlugin;
- armarx::HandUnitInterfacePrx leftHandUnit;
- armarx::HandUnitInterfacePrx rightHandUnit;
+ armarx::HandUnitInterfacePrx leftHandUnit;
+ armarx::HandUnitInterfacePrx rightHandUnit;
/// Properties shown in the Scenario GUI.
struct Properties
@@ -165,7 +165,8 @@ namespace armarx::kinesthetic_learning::components::kinesthetic_teaching
std::string rightForceTorqueSensorFrame = "FT R_ArmR_FT";
} ft;
- ::armarx::kinesthetic_learning::kinesthetic_teaching::BimanualKinestheticTeaching::Params bimanual;
+ ::armarx::kinesthetic_learning::kinesthetic_teaching::BimanualKinestheticTeaching::
+ Params bimanual;
};
Properties properties;
@@ -179,7 +180,7 @@ namespace armarx::kinesthetic_learning::components::kinesthetic_teaching
/// Tab shown in the Remote GUI.
struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
{
- struct
+ struct
{
armarx::RemoteGui::Client::Label label;
armarx::RemoteGui::Client::LineEdit annotation;
@@ -192,14 +193,14 @@ namespace armarx::kinesthetic_learning::components::kinesthetic_teaching
armarx::RemoteGui::Client::Button stopButton;
} control;
- struct HandControl
- {
+ struct HandControl
+ {
armarx::RemoteGui::Client::Button shapeButton;
armarx::RemoteGui::Client::ComboBox shapes;
- };
+ };
- HandControl leftHandControl;
- HandControl rightHandControl;
+ HandControl leftHandControl;
+ HandControl rightHandControl;
struct
{
@@ -217,7 +218,8 @@ namespace armarx::kinesthetic_learning::components::kinesthetic_teaching
RemoteGuiTab tab;
- std::optional<::armarx::kinesthetic_learning::kinesthetic_teaching::MotionRecorder> motionRecorder;
+ std::optional<::armarx::kinesthetic_learning::kinesthetic_teaching::MotionRecorder>
+ motionRecorder;
std::atomic_bool isKinestheticTeachingRunning = false;
diff --git a/source/armarx/kinesthetic_learning/components/kinesthetic_teaching/ComponentInterface.ice b/source/armarx/kinesthetic_learning/components/kinesthetic_teaching/ComponentInterface.ice
index 1a381dbbb1cc8017a9b8be06966e820cbf55a8f7..beea585e1f011b268993eaead7683bde351fb12a 100644
--- a/source/armarx/kinesthetic_learning/components/kinesthetic_teaching/ComponentInterface.ice
+++ b/source/armarx/kinesthetic_learning/components/kinesthetic_teaching/ComponentInterface.ice
@@ -26,16 +26,23 @@
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice>
-module armarx { module kinesthetic_learning { module components { module kinesthetic_teaching
+module armarx
{
+ module kinesthetic_learning
+ {
+ module components
+ {
+ module kinesthetic_teaching
+ {
- interface ComponentInterface extends
- armarx::armem::client::MemoryListenerInterface
+ interface ComponentInterface extends armarx::armem::client::MemoryListenerInterface
- {
+ {
- void startKinestheticTeaching();
- void stopKinestheticTeaching();
+ void startKinestheticTeaching();
+ void stopKinestheticTeaching();
+ };
+ };
+ };
};
-
-};};};};
+};
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/BimanualKinestheticTeaching.cpp b/source/armarx/kinesthetic_learning/kinesthetic_teaching/BimanualKinestheticTeaching.cpp
index ad88f3a6f5bed088045937811a7391722237a2b4..fb72739638e3248db7fe8864e3ec186e3feff3c3 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/BimanualKinestheticTeaching.cpp
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/BimanualKinestheticTeaching.cpp
@@ -27,11 +27,12 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
BimanualKinestheticTeaching::BimanualKinestheticTeaching(SingleArmKinestheticTeaching& left,
SingleArmKinestheticTeaching& right,
MotionRecorder& motionRecorder,
- SceneRecorder& sceneRecorder, const Params& params) :
+ SceneRecorder& sceneRecorder,
+ const Params& params) :
leftArmTeaching_(left),
rightArmTeaching_(right),
motionRecorder_(motionRecorder),
- sceneRecorder_(sceneRecorder),
+ sceneRecorder_(sceneRecorder),
params_(params)
{
}
@@ -49,38 +50,34 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
}
}
-
void
BimanualKinestheticTeaching::run()
{
// setup
- if(params_.enableLeftArm)
+ if (params_.enableLeftArm)
{
- leftKinTeachingTask_ = new armarx::SimpleRunningTask([&]() { leftArmTeaching_.run(); },
- "LeftArmKinestheticTeaching");
- motionRecorder_.registerEventChannel(&leftArmTeaching_);
-
+ leftKinTeachingTask_ = new armarx::SimpleRunningTask([&]() { leftArmTeaching_.run(); },
+ "LeftArmKinestheticTeaching");
+ motionRecorder_.registerEventChannel(&leftArmTeaching_);
}
- if(params_.enableRightArm)
+ if (params_.enableRightArm)
{
- rightKinTeachingTask_ = new armarx::SimpleRunningTask([&]() { rightArmTeaching_.run(); },
- "RightArmKinestheticTeaching");
- motionRecorder_.registerEventChannel(&rightArmTeaching_);
-
+ rightKinTeachingTask_ = new armarx::SimpleRunningTask(
+ [&]() { rightArmTeaching_.run(); }, "RightArmKinestheticTeaching");
+ motionRecorder_.registerEventChannel(&rightArmTeaching_);
}
-
// start tasks
- if(params_.enableLeftArm)
+ if (params_.enableLeftArm)
{
leftKinTeachingTask_->start();
}
- if(params_.enableRightArm)
+ if (params_.enableRightArm)
{
rightKinTeachingTask_->start();
}
@@ -92,30 +89,30 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
}
// start scene recorder
- if(not sceneRecorder_.start())
+ if (not sceneRecorder_.start())
{
ARMARX_ERROR << "Failed to start scene recorder";
return;
}
// wait until kinestetic teaching for both arms is finished
- if(params_.enableLeftArm)
+ if (params_.enableLeftArm)
{
leftKinTeachingTask_->waitForFinished();
}
- if(params_.enableRightArm)
+ if (params_.enableRightArm)
{
rightKinTeachingTask_->waitForFinished();
}
// finish / stop recording
- if(not motionRecorder_.stop())
+ if (not motionRecorder_.stop())
{
ARMARX_WARNING << "Failed to stop motion recorder!";
}
- if(not sceneRecorder_.stop())
+ if (not sceneRecorder_.stop())
{
ARMARX_WARNING << "Failed to stop scene recorder!";
}
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/BimanualKinestheticTeaching.h b/source/armarx/kinesthetic_learning/kinesthetic_teaching/BimanualKinestheticTeaching.h
index 8328c98c2ffd7ab28164afa23e0450d7785d29e7..6b7abd57d3ec403d9f264ea6607092d54dc4ca71 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/BimanualKinestheticTeaching.h
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/BimanualKinestheticTeaching.h
@@ -35,7 +35,6 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
class BimanualKinestheticTeaching
{
public:
-
struct Params
{
bool enableLeftArm = true;
@@ -45,8 +44,9 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
BimanualKinestheticTeaching(SingleArmKinestheticTeaching& left,
SingleArmKinestheticTeaching& right,
MotionRecorder& motionRecorder,
- SceneRecorder& sceneRecorder, const Params& params);
-
+ SceneRecorder& sceneRecorder,
+ const Params& params);
+
BimanualKinestheticTeaching(BimanualKinestheticTeaching&) = delete;
void run();
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/EventChannel.h b/source/armarx/kinesthetic_learning/kinesthetic_teaching/EventChannel.h
index fe054973f9fb7f65145808a676f1652f89d34113..f160d2ef711914ba2c21004e45ff83c169ee03a0 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/EventChannel.h
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/EventChannel.h
@@ -32,8 +32,7 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
class EventChannel
{
- public:
-
+ public:
virtual const std::string& name() const = 0;
struct State
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/FTWristSensor.cpp b/source/armarx/kinesthetic_learning/kinesthetic_teaching/FTWristSensor.cpp
index 269cff96928eb8bff603215f7ef6539ff19617de..e4f95c61b8e53021040eb3271d552dea522d7611 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/FTWristSensor.cpp
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/FTWristSensor.cpp
@@ -44,13 +44,12 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
{
ARMARX_VERBOSE << "Waiting for force spike";
- const auto getForce =
- [&]()
+ const auto getForce = [&]()
{
const auto timestamp = Clock::Now();
- const std::optional<
- std::map<armem::robot_state::RobotReader::Hand, armem::robot_state::proprioception::ForceTorque>>
+ const std::optional<std::map<armem::robot_state::RobotReader::Hand,
+ armem::robot_state::proprioception::ForceTorque>>
ft = robotReader.queryForceTorque(robotName, timestamp);
@@ -85,8 +84,7 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
};
- std::deque<float>
- spikes(params.windowSizeMs / params.cycleTimeMs, getForce());
+ std::deque<float> spikes(params.windowSizeMs / params.cycleTimeMs, getForce());
while (true) // stop run function if returning true
{
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/MotionRecorder.cpp b/source/armarx/kinesthetic_learning/kinesthetic_teaching/MotionRecorder.cpp
index 246750a1366e788626d3db5b248a169478c23b20..23dcc78d6ba827e15a441bced66d297b65a82689 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/MotionRecorder.cpp
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/MotionRecorder.cpp
@@ -43,9 +43,10 @@
namespace armarx::kinesthetic_learning::kinesthetic_teaching
{
- MotionRecorder::MotionRecorder(const armarx::armem::robot_state::description::RobotDescription& description,
- armarx::armem::robot_state::RobotReader& robotReader,
- const Params& params) :
+ MotionRecorder::MotionRecorder(
+ const armarx::armem::robot_state::description::RobotDescription& description,
+ armarx::armem::robot_state::RobotReader& robotReader,
+ const Params& params) :
description(description), robotReader(robotReader), params(params)
{
}
@@ -140,7 +141,7 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
timestep.robotState = robotState.value();
- if(handShapes.has_value())
+ if (handShapes.has_value())
{
timestep.handShapes = handShapes.value();
handShapes.reset();
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/MotionRecorder.h b/source/armarx/kinesthetic_learning/kinesthetic_teaching/MotionRecorder.h
index f3503de5b43a908b0e3f72d36d689d507dad7f2c..5f646a46ecdcaa8084da6313042cbba4c3850aef 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/MotionRecorder.h
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/MotionRecorder.h
@@ -29,7 +29,6 @@
#include <armarx/kinesthetic_learning/kinesthetic_teaching/EventChannel.h>
-
namespace armarx::armem::robot_state
{
class RobotReader;
@@ -48,13 +47,11 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
std::map<std::string, std::string> handShapes;
};
-
struct MotionRecorderParams
{
int recordingPeriodMs = 10;
};
-
class MotionRecorder
{
public:
@@ -99,6 +96,6 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
std::mutex handShapesMtx;
std::optional<std::map<std::string, std::string>> handShapes;
};
-
-
+
+
} // namespace armarx::kinesthetic_learning::kinesthetic_teaching
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/SceneRecorder.cpp b/source/armarx/kinesthetic_learning/kinesthetic_teaching/SceneRecorder.cpp
index c41e9b31d1a4d59ee61b1e311789dd821fa58269..5271df68daaef082202babacff0e163cc48e7f52 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/SceneRecorder.cpp
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/SceneRecorder.cpp
@@ -28,7 +28,6 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-
namespace armarx::kinesthetic_learning::kinesthetic_teaching
{
SceneRecorder::SceneRecorder(const armarx::objpose::ObjectPoseClient& objectPoseClient,
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/SceneRecorder.h b/source/armarx/kinesthetic_learning/kinesthetic_teaching/SceneRecorder.h
index ee2d3e75313876d78fdd33bb50f4f7da175ce53e..a5c97e43973c2811727146b0878bc72fa7cb981b 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/SceneRecorder.h
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/SceneRecorder.h
@@ -30,7 +30,6 @@
#include <armarx/kinesthetic_learning/kinesthetic_teaching/EventChannel.h>
-
namespace armarx::kinesthetic_learning::kinesthetic_teaching
{
@@ -40,13 +39,11 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
std::map<std::string, armarx::objpose::ObjectPoseSeq> sceneByProvider;
};
-
struct ObjectPoseRecorderParams
{
int recordingPeriodMs = 100;
};
-
class SceneRecorder
{
public:
@@ -54,7 +51,7 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
using TimeStep = SceneTimeStep;
SceneRecorder(const armarx::objpose::ObjectPoseClient& objectPoseClient,
- const Params& params = Params());
+ const Params& params = Params());
SceneRecorder(SceneRecorder&) = delete;
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/SingleArmKinestheticTeaching.cpp b/source/armarx/kinesthetic_learning/kinesthetic_teaching/SingleArmKinestheticTeaching.cpp
index ba3aa191a9d05af8693b30203993c38529dced11..c8ba0e69d16aad67b46b4d15657b42bd7a9a4b36 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/SingleArmKinestheticTeaching.cpp
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/SingleArmKinestheticTeaching.cpp
@@ -26,7 +26,6 @@
#include "armarx/kinesthetic_learning/kinesthetic_teaching/trigger/TriggerInterface.h"
#include <armarx/kinesthetic_learning/kinesthetic_teaching/ZeroTorqueController.h>
-
namespace armarx::kinesthetic_learning::kinesthetic_teaching
{
SingleArmKinestheticTeaching::SingleArmKinestheticTeaching(
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/ZeroTorqueController.cpp b/source/armarx/kinesthetic_learning/kinesthetic_teaching/ZeroTorqueController.cpp
index 670d8fdaed23b323fd88ecee9eec94271f15f339..0f5be29d2036f357c3de9c1e3121d9390cb15780 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/ZeroTorqueController.cpp
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/ZeroTorqueController.cpp
@@ -26,10 +26,8 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
-
#include <RobotAPI/interface/units/RobotUnit/NjointZeroTorqueController.h>
-
namespace armarx::kinesthetic_learning::kinesthetic_teaching
{
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/ZeroTorqueController.h b/source/armarx/kinesthetic_learning/kinesthetic_teaching/ZeroTorqueController.h
index f4c10bf70fec87e41b66e82eba720264b1900f69..7943b2f45e612a106547491f90d1001e3c9c2cd5 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/ZeroTorqueController.h
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/ZeroTorqueController.h
@@ -37,7 +37,6 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
virtual ~ZeroTorqueControllerInterface() = default;
};
-
class ZeroTorqueController : virtual public ZeroTorqueControllerInterface
{
public:
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/debug/ZeroTorqueControllerDummy.cpp b/source/armarx/kinesthetic_learning/kinesthetic_teaching/debug/ZeroTorqueControllerDummy.cpp
index 380622fce8aef6bc8f01113fa55eff93c947fba4..b655201843cfdd1abf45e74d3d9deb63e251fc3b 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/debug/ZeroTorqueControllerDummy.cpp
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/debug/ZeroTorqueControllerDummy.cpp
@@ -29,7 +29,6 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching::debug
{
}
-
void
ZeroTorqueControllerDummy::disable()
{
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/json_conversions.cpp b/source/armarx/kinesthetic_learning/kinesthetic_teaching/json_conversions.cpp
index 866848b727a1ed481df5197fcf09dfde01e06494..066c8a4ce33e61db453baccd978544ed731e2d21 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/json_conversions.cpp
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/json_conversions.cpp
@@ -1,19 +1,18 @@
#include "json_conversions.h"
-#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
+#include <fstream>
#include <SimoxUtility/json/eigen_conversion.h>
#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/XML/RobotIO.h>
#include "ArmarXCore/core/time/Duration.h"
-#include "armarx/kinesthetic_learning/kinesthetic_teaching/EventChannel.h"
-#include "armarx/kinesthetic_learning/kinesthetic_teaching/MotionRecorder.h"
-
+#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <fstream>
+#include "armarx/kinesthetic_learning/kinesthetic_teaching/EventChannel.h"
+#include "armarx/kinesthetic_learning/kinesthetic_teaching/MotionRecorder.h"
namespace armarx::armem::robot_state
{
@@ -33,7 +32,7 @@ namespace armarx::armem::robot_state
j["jointMap"] = bo.jointMap;
j["timestamp"] = bo.timestamp.toMicroSecondsSinceEpoch();
}
-} // namespace armarx::armem::robot
+} // namespace armarx::armem::robot_state
namespace armarx::kinesthetic_learning::kinesthetic_teaching
{
@@ -58,7 +57,6 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
bo.handShapes = j["handShapes"].get<decltype(bo.handShapes)>();
}
-
void
to_json(nlohmann::json& j, const MotionTimeStep& bo)
{
@@ -67,7 +65,6 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
j["handShapes"] = bo.handShapes;
}
-
void
from_json(const nlohmann::json& j, SceneTimeStep& bo)
{
@@ -75,7 +72,6 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
bo.sceneByProvider = j["sceneByProvider"].get<decltype(bo.sceneByProvider)>();
}
-
void
to_json(nlohmann::json& j, const SceneTimeStep& bo)
{
@@ -83,7 +79,8 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
j["sceneByProvider"] = bo.sceneByProvider;
}
- std::map<double, std::vector<double> > getTrajDataFromJson(std::string filepath, std::string side)
+ std::map<double, std::vector<double>>
+ getTrajDataFromJson(std::string filepath, std::string side)
{
std::map<double, std::vector<double>> trajData;
nlohmann::json kinestheticTeachingData;
@@ -104,9 +101,7 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
// attention: currently hardcoded to armar 6
std::string robotName;
- ArmarXDataPath::getAbsolutePath(
- "armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml",
- robotName);
+ ArmarXDataPath::getAbsolutePath("armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml", robotName);
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotName);
RobotNameHelperPtr rnh = RobotNameHelper::Create(robotName);
@@ -117,12 +112,13 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
{
auto robotStates = kinestheticTeachingData["motion"];
- for (const auto& item: robotStates)
+ for (const auto& item : robotStates)
{
auto robotStateJson = item.at("robotState");
armarx::armem::robot_state::RobotState robotState;
armarx::armem::robot_state::from_json(robotStateJson, robotState);
- if(!firstTimestepSet){
+ if (!firstTimestepSet)
+ {
firstTimeStep = robotState.timestamp;
firstTimestepSet = true;
}
@@ -143,9 +139,10 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
std::vector<double> poseVec;
for (size_t i = 0; i < 3; ++i)
{
- poseVec.push_back(poseTCP.coeff(i,3));
+ poseVec.push_back(poseTCP.coeff(i, 3));
}
- VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(poseTCP);
+ VirtualRobot::MathTools::Quaternion quat =
+ VirtualRobot::MathTools::eigen4f2quat(poseTCP);
poseVec.push_back(quat.w);
poseVec.push_back(quat.x);
poseVec.push_back(quat.y);
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/json_conversions.h b/source/armarx/kinesthetic_learning/kinesthetic_teaching/json_conversions.h
index 0b64ec65a3144148d68e43e4c5081dc1d6fdc23c..27b7e4a33f5c809587797bca56f0cedaa751e632 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/json_conversions.h
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/json_conversions.h
@@ -31,7 +31,8 @@ namespace armarx::armem::robot_state
void from_json(const nlohmann::json& j, RobotState& bo);
void to_json(nlohmann::json& j, const RobotState& bo);
-}
+} // namespace armarx::armem::robot_state
+
namespace armarx::kinesthetic_learning::kinesthetic_teaching
{
@@ -41,5 +42,6 @@ namespace armarx::kinesthetic_learning::kinesthetic_teaching
void from_json(const nlohmann::json& j, SceneTimeStep& bo);
void to_json(nlohmann::json& j, const SceneTimeStep& bo);
- std::map<double, std::vector<double>> getTrajDataFromJson(std::string filepath, std::string side_rns);
+ std::map<double, std::vector<double>> getTrajDataFromJson(std::string filepath,
+ std::string side_rns);
} // namespace armarx::kinesthetic_learning::kinesthetic_teaching
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/kinesthetic_teaching.h b/source/armarx/kinesthetic_learning/kinesthetic_teaching/kinesthetic_teaching.h
index 19eacffc73956c78b16133e2c1e64e06f043d645..a3b584cfd2ce198e12ca80bbbea05f68dd4c8740 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/kinesthetic_teaching.h
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/kinesthetic_teaching.h
@@ -22,7 +22,6 @@
#pragma once
-
namespace armarx
{
/**
@@ -39,7 +38,6 @@ namespace armarx
class kinesthetic_teaching
{
public:
-
};
-}
+} // namespace armarx
diff --git a/source/armarx/kinesthetic_learning/kinesthetic_teaching/test/kinesthetic_teachingTest.cpp b/source/armarx/kinesthetic_learning/kinesthetic_teaching/test/kinesthetic_teachingTest.cpp
index 05ee9ec44905828f17bfeafddb1aef908e4f16c1..4afc7e9c103733dd4087c624792d4b9e95883fe0 100644
--- a/source/armarx/kinesthetic_learning/kinesthetic_teaching/test/kinesthetic_teachingTest.cpp
+++ b/source/armarx/kinesthetic_learning/kinesthetic_teaching/test/kinesthetic_teachingTest.cpp
@@ -24,11 +24,12 @@
#define ARMARX_BOOST_TEST
-#include <armarx/kinesthetic_learning/Test.h>
#include "../kinesthetic_teaching.h"
#include <iostream>
+#include <armarx/kinesthetic_learning/Test.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
diff --git a/source/armarx/kinesthetic_learning/memory/client/demonstration/Reader.cpp b/source/armarx/kinesthetic_learning/memory/client/demonstration/Reader.cpp
index db3131c7b97fe07a87e60aa09ecb4f52dbca48d5..9638cfe054847f6a2f0dec109db70c4139d210c1 100644
--- a/source/armarx/kinesthetic_learning/memory/client/demonstration/Reader.cpp
+++ b/source/armarx/kinesthetic_learning/memory/client/demonstration/Reader.cpp
@@ -20,6 +20,7 @@
*/
#include "Reader.h"
+
#include "ArmarXCore/util/CPPUtility/trace.h"
#include <RobotAPI/libraries/armem/client/query/Builder.h>
@@ -49,7 +50,7 @@ namespace armarx::kinesthetic_learning::memory::client::demonstration
asDemonstration(const armem::wm::ProviderSegment& providerSegment)
{
ARMARX_TRACE;
-
+
ARMARX_CHECK(not providerSegment.empty()) << "No entities";
ARMARX_CHECK_EQUAL(providerSegment.size(), 1) << "There should be only one entity!";
diff --git a/source/armarx/kinesthetic_learning/memory/client/demonstration/Reader.h b/source/armarx/kinesthetic_learning/memory/client/demonstration/Reader.h
index 0b8dcefb1b129db4b069100c00094b27c8881722..bce4643bc2f85f231c1637ea2f3b484e1c46e9fc 100644
--- a/source/armarx/kinesthetic_learning/memory/client/demonstration/Reader.h
+++ b/source/armarx/kinesthetic_learning/memory/client/demonstration/Reader.h
@@ -32,6 +32,7 @@ namespace armarx::kinesthetic_learning::memory::client::demonstration
{
public:
using armarx::armem::client::util::SimpleReaderBase::SimpleReaderBase;
+
// ~Reader() override;
struct Query