diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml index ef43ecebfd31e5a6e7e8db6ac1fa89acfb06e715..9d57e40ea3d4b9902bc04738f66cb15ab532f233 100644 --- a/data/RobotAPI/VariantInfo-RobotAPI.xml +++ b/data/RobotAPI/VariantInfo-RobotAPI.xml @@ -267,17 +267,18 @@ <Proxy include="RobotAPI/interface/components/ObstacleAvoidance/ObstacleAvoidanceInterface.h" humanName="Platform obstacle avoidance interface" typeName="ObstacleAvoidanceInterfacePrx" - memberName="platformObstacleAvoidance" - getterName="getPlatformObstacleAvoidance" - propertyName="PlatformObstacleAvoidanceName" + memberName="obstacleAvoidance" + getterName="getObstacleAvoidance" + propertyName="ObstacleAvoidanceName" propertyIsOptional="true" propertyDefaultValue="PlatformObstacleAvoidance" /> - <Proxy include="RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectorInterface.h" + <Proxy include="RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.h" humanName="Platform obstacle detection interface" typeName="ObstacleDetectionInterfacePrx" - memberName="platformObstacleDetection" - getterName="getPlatformObstacleDetection" - propertyName="PlatformObstacleDetectionName" + memberName="obstacleDetection" + getterName="getObstacleDetection" + propertyName="ObstacleDetectionName" + propertyIsOptional="true" propertyDefaultValue="PlatformObstacleAvoidance" /> <Proxy include="RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h" humanName="Dynamic obstacle manager interface" @@ -285,6 +286,7 @@ memberName="dynamicObstacleManager" getterName="getDynamicObstacleManager" propertyName="DynamicObstacleManagerName" + propertyIsOptional="true" propertyDefaultValue="DynamicObstacleManager" /> <Proxy include="RobotAPI/interface/observers/GraspCandidateObserverInterface.h" humanName="Grasp Candidate Observer" diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp index d1e10e97f8a89dafba0671a704f5814f5ca5ca84..3a066c978c6464055b8968e9c8c893b6b91412f4 100644 --- a/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp +++ b/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp @@ -9,6 +9,8 @@ #include <VirtualRobot/XML/ObjectIO.h> #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h> +#include <boost/algorithm/string/predicate.hpp> + namespace armarx::viz::coin { @@ -46,7 +48,19 @@ namespace armarx::viz::coin try { ARMARX_INFO << "Loading object from " << fullFilename; - result = VirtualRobot::ObjectIO::loadManipulationObject(fullFilename); + + if (boost::ends_with(fullFilename, ".wrl")) + { + VirtualRobot::VisualizationFactoryPtr factory = VirtualRobot::VisualizationFactory::fromName("inventor", NULL); + VirtualRobot::VisualizationNodePtr vis = factory->getVisualizationFromFile(fullFilename); + result = VirtualRobot::ManipulationObjectPtr(new VirtualRobot::ManipulationObject(filename, vis)); + + } + else + { + result = VirtualRobot::ObjectIO::loadManipulationObject(fullFilename); + + } } catch (std::exception const& ex) { @@ -55,6 +69,7 @@ namespace armarx::viz::coin } + return result; } diff --git a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp index ee891427aebe42fd391a64efee478ab2e9b260f0..4a83bdedaabbabcc2f46bc4e2815a826ee661156 100644 --- a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp +++ b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp @@ -316,22 +316,22 @@ armarx::DSObstacleAvoidance::removeObstacle(const std::string& obstacle_name, co std::vector<armarx::obstacledetection::Obstacle> armarx::DSObstacleAvoidance::getObstacles(const Ice::Current&) { + ARMARX_DEBUG << "Get Obstacles"; std::shared_lock l{m_doa.mutex}; std::vector<obstacledetection::Obstacle> obstacle_list; for (auto& [doa_name, doa_obstacle] : m_doa.env) { - std::shared_ptr<doa::Ellipsoid> doa_ellipsoid = - std::dynamic_pointer_cast<doa::Ellipsoid>(doa_obstacle); + ARMARX_DEBUG << "Got an obtascle"; + std::shared_ptr<doa::Ellipsoid> doa_ellipsoid = std::dynamic_pointer_cast<doa::Ellipsoid>(doa_obstacle); obstacledetection::Obstacle obstacle; obstacle.name = doa_name; obstacle.posX = doa_ellipsoid->get_position()(0) * 1000; obstacle.posY = doa_ellipsoid->get_position()(1) * 1000; obstacle.posZ = doa_ellipsoid->get_position()(2) * 1000; - obstacle.yaw = simox::math::mat3f_to_rpy(doa_ellipsoid->get_orientation() - .toRotationMatrix().cast<float>()).z(); + obstacle.yaw = simox::math::mat3f_to_rpy(doa_ellipsoid->get_orientation().toRotationMatrix().cast<float>()).z(); obstacle.axisLengthX = doa_ellipsoid->get_axis_lengths(0) * 1000; obstacle.axisLengthY = doa_ellipsoid->get_axis_lengths(1) * 1000; obstacle.axisLengthZ = doa_ellipsoid->get_axis_lengths(2) * 1000; @@ -345,9 +345,11 @@ armarx::DSObstacleAvoidance::getObstacles(const Ice::Current&) obstacle.curvatureY = doa_ellipsoid->get_curvature_factor()(1); obstacle.curvatureZ = doa_ellipsoid->get_curvature_factor()(2); + ARMARX_DEBUG << "push back " << obstacle.name; obstacle_list.push_back(std::move(obstacle)); } + ARMARX_DEBUG << "Return obstacle list"; return obstacle_list; } @@ -357,6 +359,8 @@ armarx::DSObstacleAvoidance::modulateVelocity( const obstacleavoidance::Agent& agent, const Ice::Current&) { + + ARMARX_DEBUG << "Modulate velocity"; std::shared_lock l{m_doa.mutex}; // Create and initialize agent. @@ -404,7 +408,7 @@ void armarx::DSObstacleAvoidance::updateEnvironment(const Ice::Current&) { ARMARX_VERBOSE << "Updating environment."; - + std::lock_guard l2{m_vis.mutex}; std::lock_guard l{m_doa.mutex}; // Make working copies of the updates and free them again. @@ -471,7 +475,6 @@ armarx::DSObstacleAvoidance::updateEnvironment(const Ice::Current&) m_doa.env.update(); { - std::lock_guard l{m_vis.mutex}; m_vis.needs_update = true; } @@ -483,6 +486,7 @@ armarx::DSObstacleAvoidance::updateEnvironment(const Ice::Current&) void armarx::DSObstacleAvoidance::writeDebugPlots(const std::string& filename, const Ice::Current&) { + ARMARX_DEBUG << "Write debug plots"; std::shared_lock l{m_doa.mutex}; const std::string filename_annotated = getName() + "_" + filename; @@ -532,6 +536,7 @@ const void armarx::DSObstacleAvoidance::run_visualization() { + ARMARX_DEBUG << "Run visualization"; using namespace armarx::viz; std::lock_guard l{m_vis.mutex}; diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp index ccc5409fc336854e175d0245f7a24a5b973f5bb7..46515a2c2f9a61acbaf63c186920478dab82cb1c 100644 --- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp +++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp @@ -143,6 +143,7 @@ namespace armarx // First check own obstacles for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles) { + std::lock_guard<std::shared_mutex> l(managed_obstacle->m_mutex); float coverage = ManagedObstacle::line_segment_ellipsis_coverage( {managed_obstacle->m_obstacle.posX, managed_obstacle->m_obstacle.posY}, managed_obstacle->m_obstacle.axisLengthX, managed_obstacle->m_obstacle.axisLengthY, managed_obstacle->m_obstacle.yaw, @@ -219,16 +220,16 @@ namespace armarx ARMARX_DEBUG << "Remove Obstacle " << name << " from obstacle map and from obstacledetection"; for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles) { - if(managed_obstacle->m_obstacle.name == name) + if (managed_obstacle->m_obstacle.name == name) { managed_obstacle->m_value = std::numeric_limits<double>::min(); managed_obstacle->m_updated = true; managed_obstacle->m_published = false; - m_obstacle_detection->removeObstacle(name); - m_obstacle_detection->updateEnvironment(); - return; // We assume unique names + break; } } + m_obstacle_detection->removeObstacle(name); + m_obstacle_detection->updateEnvironment(); } void DynamicObstacleManager::wait_unitl_obstacles_are_ready(const Ice::Current&) @@ -257,6 +258,7 @@ namespace armarx void DynamicObstacleManager::update_decayable_obstacles() { + ARMARX_DEBUG << "update obstacles"; IceUtil::Time started = IceUtil::Time::now(); std::vector<std::string> remove_obstacles; @@ -265,6 +267,7 @@ namespace armarx // Update positions { + ARMARX_DEBUG << "update obstacle position"; std::shared_lock<std::shared_mutex> l(m_managed_obstacles_mutex); if (!m_managed_obstacles.size()) { @@ -285,6 +288,7 @@ namespace armarx int published_obstacles = 0; int updated_obstacles = 0; { + ARMARX_DEBUG << "update obstacle values and publish"; std::lock_guard l(m_managed_obstacles_mutex); std::sort(m_managed_obstacles.begin(), m_managed_obstacles.end(), ManagedObstacle::ComparatorDESCPrt); @@ -366,6 +370,7 @@ namespace armarx if (updated) { + ARMARX_DEBUG << "update environment"; m_obstacle_detection->updateEnvironment(); } arviz.commit({obstacleLayer}); diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp index 57bd492d1e56a1e54513c4af3ac5431b1ff68e25..b7d21d33b49202aa16928be602d306ed522ad9a1 100644 --- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp +++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp @@ -26,6 +26,8 @@ + + namespace armarx { void GamepadControlUnit::onInitComponent() @@ -33,12 +35,14 @@ namespace armarx ARMARX_INFO << "oninit GamepadControlUnit"; usingProxy(getProperty<std::string>("PlatformUnitName").getValue()); usingTopic(getProperty<std::string>("GamepadTopicName").getValue()); - + usingProxy("EmergencyStopMaster"); scaleX = getProperty<float>("ScaleX").getValue(); scaleY = getProperty<float>("ScaleY").getValue(); scaleRotation = getProperty<float>("ScaleAngle").getValue(); ARMARX_INFO << "oninit GamepadControlUnit end"; + + } @@ -46,6 +50,7 @@ namespace armarx { ARMARX_INFO << "onConnect GamepadControlUnit"; platformUnitPrx = getProxy<PlatformUnitInterfacePrx>(getProperty<std::string>("PlatformUnitName").getValue()); + emergencyStop = getProxy<EmergencyStopMasterInterfacePrx>("EmergencyStopMaster"); } @@ -68,6 +73,10 @@ namespace armarx void GamepadControlUnit::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c) { + if (data.leftTrigger > 0) + { + emergencyStop->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive); + } //scales are for the robot axis if (data.rightTrigger > 0) { diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h index 2614a96fff87084ee3383bd705c05e6c2c6bbc5f..abdf77a77d42aa73a48bce6c93a15416bb8c64bc 100644 --- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h +++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h @@ -24,6 +24,7 @@ #include <ArmarXCore/core/Component.h> +#include <ArmarXCore/interface/components/EmergencyStopInterface.h> #include <RobotAPI/interface/units/GamepadUnit.h> @@ -105,6 +106,7 @@ namespace armarx float scaleX; float scaleY; float scaleRotation; + EmergencyStopMasterInterfacePrx emergencyStop; public: void reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp index 2e5fb58833964524fde25b30376cb22c125002ce..f7d2eee97b3ec601420ce9cf010af23817814dd7 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp +++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp @@ -53,6 +53,8 @@ namespace armarx defs->optional(visu.enabled, "visu.enabled", "Enable or disable visualization of objects."); defs->optional(visu.inGlobalFrame, "visu.inGlobalFrame", "If true, show global poses. If false, show poses in robot frame."); defs->optional(visu.alpha, "visu.alpha", "Alpha of objects (1 = solid, 0 = transparent)."); + defs->optional(calibration.robotNode, "calibration.robotNode", "Robot node which can be calibrated"); + defs->optional(calibration.offset, "calibration.offset", "Offset for the node to be calibrated"); return defs; } @@ -143,6 +145,13 @@ namespace armarx std::scoped_lock lock(dataMutex); RobotState::synchronizeLocalClone("robot"); + if (robot->hasRobotNode(calibration.robotNode)) + { + VirtualRobot::RobotNodePtr robotNode = robot->getRobotNode(calibration.robotNode); + float value = robotNode->getJointValue(); + robotNode->setJointValue(value + calibration.offset); + } + for (const auto& provided : providedPoses) { objpose::ObjectPose& pose = objectPoses.emplace_back(); @@ -161,6 +170,7 @@ namespace armarx } } + void ObjectPoseObserver::fetchProviderInfo(const std::string& providerName, bool refresh) { { diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h index b372887cf6e9d0b587817986081bf83cff75a448..0c631b1e65bffbfd1199abccc016e98c5f1be724 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h +++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h @@ -154,6 +154,13 @@ namespace armarx }; Visu visu; + struct Calibration + { + std::string robotNode = "Neck_2_Pitch"; + float offset = 0.0f; + }; + Calibration calibration; + struct RemoteGuiTab : RemoteGui::Client::Tab { diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h index b4aced7b9e0fb437985e14d8bf8a2fa1adfb9440..71c9f1caad2438d2c0869dc64e70f01440907de6 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h @@ -92,6 +92,11 @@ namespace armarx::RobotUnitModule } void aggregatedHeartbeat(RobotHealthState overallHealthState, const Ice::Current&) override; + bool isSimulation(const Ice::Current& = Ice::emptyCurrent) const override + { + return false; + } + // //////////////////////////////////////////////////////////////////////////////////////// // // ///////////////////////////////////////// Data ///////////////////////////////////////// // // //////////////////////////////////////////////////////////////////////////////////////// // diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp index 6732b5b66028b2c9b1389088a36ead1ae1147093..37c660527f62b0e50549e6297ec9d31fc06ab350 100644 --- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp +++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp @@ -85,12 +85,14 @@ namespace armarx // Layer info tree. connect(widget.layerTree, &QTreeWidget::currentItemChanged, this, &This::updateSelectedLayer); +#if 0 connect(widget.defaultShowLimitSpinBox, qOverload<int>(&QSpinBox::valueChanged), &layerInfoTree, &LayerInfoTree::setMaxElementCountDefault); layerInfoTree.setMaxElementCountDefault(widget.defaultShowLimitSpinBox->value()); layerInfoTree.setWidget(widget.layerInfoTree); layerInfoTree.registerVisualizerCallbacks(visualizer); +#endif // We need a callback from the visualizer, when the layers have changed @@ -289,6 +291,7 @@ namespace armarx void ArVizWidgetController::updateSelectedLayer(QTreeWidgetItem* current, QTreeWidgetItem* previous) { +#if 0 (void) previous; if (!current->parent()) @@ -309,6 +312,7 @@ namespace armarx { ARMARX_WARNING << "Could not find layer (" << id.first << " / " << id.second << ") in Visualizer."; } +#endif } void ArVizWidgetController::onCollapseAll(bool) @@ -650,10 +654,17 @@ namespace armarx { updates[updateIter->update.name] = &updateIter->update; } + + auto layerIDsBefore = visualizer.getLayerIDs(); for (auto& pair : updates) { visualizer.apply(*pair.second); } + auto layerIDsAfter = visualizer.getLayerIDs(); + if (layerIDsAfter != layerIDsBefore) + { + visualizer.emitLayersChanged(layerIDsAfter); + } return updateBegin->timestampInMicroseconds; } diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h index f133beec6a99782c94321d108f3972030c3e6d7f..383bfd544aff904db1afe911363613dfa2dcf269 100644 --- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h +++ b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h @@ -98,7 +98,7 @@ namespace armarx */ static QString GetWidgetName() { - return "RobotControl.CartesianNaturalPositionController"; + return "RobotControl.NJointControllers.CartesianNaturalPositionController"; } void onInitComponent() override; diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h index 47eb81db0e3eb8e4c3c459ac9b50b83054f941cc..ae6961634e680913480e1441c572af6f397aa15f 100644 --- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h +++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h @@ -66,13 +66,11 @@ namespace armarx public: /// Controller Constructor explicit CartesianWaypointControlGuiWidgetController(); - /// Controller destructor virtual ~CartesianWaypointControlGuiWidgetController(); /// @see ArmarXWidgetController::loadSettings() void loadSettings(QSettings* settings) override; - /// @see ArmarXWidgetController::saveSettings() void saveSettings(QSettings* settings) override; @@ -82,7 +80,7 @@ namespace armarx */ static QString GetWidgetName() { - return "RobotControl.CartesianWaypointControlGui"; + return "RobotControl.NJointControllers.CartesianWaypointControlGui"; } void onInitComponent() override; @@ -92,9 +90,6 @@ namespace armarx QPointer<QDialog> getConfigDialog(QWidget* parent) override; void configured() override; - signals: - /* QT signal declarations */ - private slots: void on_pushButtonStop_clicked(); void on_pushButtonExecute_clicked(); diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt index c13ab4dcd519f7a19aec3f2af919b8a0a793604f..ffc177c8b38ff3f9e0569afba0ec2000070f7753 100644 --- a/source/RobotAPI/interface/CMakeLists.txt +++ b/source/RobotAPI/interface/CMakeLists.txt @@ -87,6 +87,9 @@ set(SLICE_FILES components/TrajectoryPlayerInterface.ice components/ViewSelectionInterface.ice + components/CartesianPositionControlInterface.ice + components/NaturalIKInterface.ice + visualization/DebugDrawerInterface.ice visualization/DebugDrawerToArViz.ice diff --git a/source/RobotAPI/interface/aron.ice b/source/RobotAPI/interface/aron.ice index 5ad2c13812ee9ddf3e29a39cfa2f82a51669540f..3dbe06245692e4392a05e9d2b9997dfe1dac09bf 100644 --- a/source/RobotAPI/interface/aron.ice +++ b/source/RobotAPI/interface/aron.ice @@ -16,10 +16,12 @@ module aron sequence<AronKeyValuePair> AronKeyValueList; sequence<AronData> AronDataList; sequence<byte> AronBlobValue; + sequence<int> AronIntSeq; - class AronList extends AronData { AronDataList elements; }; - class AronObject extends AronData { AronKeyValueList elements; }; + class AronList extends AronData { AronDataList elements; }; + class AronDict extends AronData { AronKeyValueList elements; }; class AronNull extends AronData { }; + class AronNDArray extends AronData { AronIntSeq dimensions; int itemSize; string type; AronBlobValue data; }; #define HANDLE_TYPE(cppType, upperType) \ class Aron##upperType extends AronData { cppType value; }; @@ -30,36 +32,10 @@ module aron HANDLE_TYPE(float, Float) HANDLE_TYPE(double, Double) HANDLE_TYPE(bool, Bool) - HANDLE_TYPE(AronBlobValue, Blob) - - HANDLE_TYPE(Eigen::Vector2f, Vector2f) - HANDLE_TYPE(Eigen::Vector3f, Vector3f) - HANDLE_TYPE(Eigen::Vector6f, Vector6f) - HANDLE_TYPE(Eigen::VectorXf, VectorXf) - HANDLE_TYPE(Eigen::Matrix3f, Matrix3f) - HANDLE_TYPE(Eigen::Matrix4f, Matrix4f) - HANDLE_TYPE(Eigen::MatrixXf, MatrixXf) + #undef HANDLE_TYPE - //class AronString extends AronData { string value; }; - //class AronInt extends AronData { int value; }; - //class AronLong extends AronData { long value; }; - //class AronFloat extends AronData { float value; }; - //class AronDouble extends AronData { double value; }; - //class AronBool extends AronData { bool value; }; - //class AronBlob extends AronData { AronBlobValue value; }; - //class AronNull extends AronData { }; - // - //class AronVector2f extends AronData { Eigen::Vector2f value; }; - //class AronVector3f extends AronData { Eigen::Vector3f value; }; - //class AronVector6f extends AronData { Eigen::Vector6f value; }; - //class AronVectorXf extends AronData { Eigen::VectorXf value; }; - // - //class AronMatrix3f extends AronData { Eigen::Matrix3f value; }; - //class AronMatrix4f extends AronData { Eigen::Matrix4f value; }; - //class AronMatrixXf extends AronData { Eigen::MatrixXf value; }; - //class AronQuaternionf extends AronData { Eigen::Quaternionf value; }; module types { @@ -73,7 +49,7 @@ module aron class ListType extends AbstractType { AbstractType childtype; }; - class ObjectType extends AbstractType { + class DictType extends AbstractType { AbstractType childtype; }; diff --git a/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice b/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice new file mode 100644 index 0000000000000000000000000000000000000000..13741869c90df9117d6f326036ae92b6e6a24ce0 --- /dev/null +++ b/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice @@ -0,0 +1,42 @@ +/* + * This file is part of ArmarX. + * + * ArmarX is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * ArmarX is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu ) + * @date 2019 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + + +#pragma once + +#include <ArmarXCore/interface/core/BasicVectorTypes.ice> +#include <ArmarXCore/interface/serialization/Eigen.ice> +//#include <RobotAPI/interface/aron.h> + +module armarx +{ + interface CartesianPositionControlInterface + { + void setEnabled(string side, bool enabled); + void setTarget(string side, Eigen::Matrix4f target, bool setOrientation); + void setFTLimit(string side, float maxForce, float maxTorque); + void setCurrentFTAsOffset(string side); + void resetFTOffset(string side); + int getFTTresholdExceededCounter(string side); + void emulateFTTresholdExceeded(string side); + + }; +}; diff --git a/source/RobotAPI/interface/components/NaturalIKInterface.ice b/source/RobotAPI/interface/components/NaturalIKInterface.ice new file mode 100644 index 0000000000000000000000000000000000000000..a5627f61ef822deaa31e59eb065a2032a877ddcd --- /dev/null +++ b/source/RobotAPI/interface/components/NaturalIKInterface.ice @@ -0,0 +1,41 @@ +/* + * This file is part of ArmarX. + * + * ArmarX is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * ArmarX is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu ) + * @date 2019 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + + +#pragma once + +#include <ArmarXCore/interface/core/BasicVectorTypes.ice> +#include <ArmarXCore/interface/serialization/Eigen.ice> +#include <RobotAPI/interface/aron.ice> + +module armarx +{ + struct NaturalIKResult + { + bool reached; + Ice::FloatSeq jointValues; + }; + + interface NaturalIKInterface + { + NaturalIKResult solveIK(string side, Eigen::Matrix4f target, bool setOrientation, aron::AronDict args); + }; +}; diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice index 04176dc066828cfc28577851d556464880db076d..42ed92db3ba81fe2a0255bab1e9a98c9f386991a 100644 --- a/source/RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice +++ b/source/RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice @@ -56,8 +56,7 @@ module armarx ObstacleAvoidanceInterface, ObstacleDetectionInterface { - dsobstacleavoidance::Config - getConfig(); + dsobstacleavoidance::Config getConfig(); void writeDebugPlots(string filename); diff --git a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice index 8749be775e87d64a4538fcfae4e2e1079e478612..ac4da1da07d1ac2c75526a2f98c32c547aa3d88d 100644 --- a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice +++ b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice @@ -62,6 +62,7 @@ module armarx ApertureType preshape = AnyAperture; ApproachType approach = AnyApproach; string graspTrajectoryName; + //string graspTrajectoryPackage; }; class GraspCandidateReachabilityInfo { diff --git a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice index 8f3237b5312f56a01c71acc49d3bd7bbad401337..361cf690a3498a2a9ffcb95527ef27005c54fe86 100644 --- a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice +++ b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice @@ -187,6 +187,7 @@ module armarx { //state ["cpp:const"] idempotent bool isRunning(); + ["cpp:const"] idempotent bool isSimulation(); }; interface RobotUnitLoggingInterface { diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXEtherCAT/CMakeLists.txt index f653b4205b8c3e2e4af3ff3a1472d65e90bac3a8..3e54ff4f9b66febc9f1403e62fa194262850a275 100644 --- a/source/RobotAPI/libraries/ArmarXEtherCAT/CMakeLists.txt +++ b/source/RobotAPI/libraries/ArmarXEtherCAT/CMakeLists.txt @@ -35,6 +35,7 @@ armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}") if (SOEM_FOUND) target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${SOEM_INCLUDE_DIR}) + target_link_libraries("${LIB_NAME}" PUBLIC ${SOEM_LIBRARIES}) endif() # add unit tests add_subdirectory(test) diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt index 1292e78fa38178ff7f555aae561465bbb4e27ac6..6567872ebd97ab3016b3af816f2048d473040464 100644 --- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt @@ -13,11 +13,27 @@ set(LIB_FILES RobotStateComponentPlugin.cpp DebugDrawerHelperComponentPlugin.cpp ArVizComponentPlugin.cpp + GraspCandidateObserverComponentPlugin.cpp + PlatformUnitComponentPlugin.cpp + RobotUnitComponentPlugin.cpp + RobotUnitObserverComponentPlugin.cpp + KinematicUnitComponentPlugin.cpp + TrajectoryPlayerComponentPlugin.cpp + CartesianPositionControlComponentPlugin.cpp + NaturalIKComponentPlugin.cpp ) set(LIB_HEADERS RobotStateComponentPlugin.h DebugDrawerHelperComponentPlugin.h ArVizComponentPlugin.h + GraspCandidateObserverComponentPlugin.h + PlatformUnitComponentPlugin.h + RobotUnitComponentPlugin.h + RobotUnitObserverComponentPlugin.h + KinematicUnitComponentPlugin.h + TrajectoryPlayerComponentPlugin.h + CartesianPositionControlComponentPlugin.h + NaturalIKComponentPlugin.h ) armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}") diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6b403e45ccfea923d1ead7b8e73fa5e7dbcf0de2 --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp @@ -0,0 +1,53 @@ +#include "CartesianPositionControlComponentPlugin.h" + +namespace armarx +{ + namespace plugins + { + + void CartesianPositionControlComponentPlugin::preOnInitComponent() + { + parent<Component>().usingProxyFromProperty(PROPERTY_NAME); + } + + void CartesianPositionControlComponentPlugin::preOnConnectComponent() + { + parent<Component>().getProxyFromProperty(_cartesianPositionControl, PROPERTY_NAME); + } + + void CartesianPositionControlComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties) + { + if (!properties->hasDefinition(PROPERTY_NAME)) + { + properties->defineRequiredProperty<std::string>( + PROPERTY_NAME, + "Name of the CartesianPositionControl"); + } + } + + CartesianPositionControlInterfacePrx CartesianPositionControlComponentPlugin::getCartesianPositionControl() + { + return _cartesianPositionControl; + } + + + } +} + +namespace armarx +{ + + CartesianPositionControlComponentPluginUser::CartesianPositionControlComponentPluginUser() + { + addPlugin(plugin); + } + + CartesianPositionControlInterfacePrx CartesianPositionControlComponentPluginUser::getCartesianPositionControl() + { + return plugin->getCartesianPositionControl(); + } + + +} + + diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h new file mode 100644 index 0000000000000000000000000000000000000000..0d53885cc44d3d0d42f4357c6c7b7cf0cfa7db5f --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h @@ -0,0 +1,58 @@ +#pragma once + +#include <ArmarXCore/core/Component.h> +#include <RobotAPI/interface/components/CartesianPositionControlInterface.h> + + +namespace armarx +{ + namespace plugins + { + + class CartesianPositionControlComponentPlugin : public ComponentPlugin + { + public: + using ComponentPlugin::ComponentPlugin; + + void preOnInitComponent() override; + + void preOnConnectComponent() override; + + void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override; + + CartesianPositionControlInterfacePrx getCartesianPositionControl(); + + private: + static constexpr const char* PROPERTY_NAME = "CartesianPositionControlName"; + CartesianPositionControlInterfacePrx _cartesianPositionControl; + }; + + } +} + +namespace armarx +{ + /** + * @brief Provides a ready-to-use CartesianPositionControl. + */ + class CartesianPositionControlComponentPluginUser : virtual public ManagedIceObject + { + public: + CartesianPositionControlComponentPluginUser(); + CartesianPositionControlInterfacePrx getCartesianPositionControl(); + + private: + armarx::plugins::CartesianPositionControlComponentPlugin* plugin = nullptr; + }; + +} + + +namespace armarx +{ + namespace plugins + { + // Legacy typedef. + using CartesianPositionControlComponentPluginUser = armarx::CartesianPositionControlComponentPluginUser; + } +} diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3e99ccfb4eb2994373380b03415f0e96b39b0e14 --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp @@ -0,0 +1,54 @@ +#include "GraspCandidateObserverComponentPlugin.h" + +namespace armarx +{ + namespace plugins + { + + void GraspCandidateObserverComponentPlugin::preOnInitComponent() + { + parent<Component>().usingProxyFromProperty(PROPERTY_NAME); + } + + void GraspCandidateObserverComponentPlugin::preOnConnectComponent() + { + parent<Component>().getProxyFromProperty(_graspCandidateObserver, PROPERTY_NAME); + } + + void GraspCandidateObserverComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties) + { + if (!properties->hasDefinition(PROPERTY_NAME)) + { + properties->defineOptionalProperty<std::string>( + PROPERTY_NAME, + "GraspCandidateObserver", + "Name of the GraspCandidateObserver"); + } + } + + grasping::GraspCandidateObserverInterfacePrx GraspCandidateObserverComponentPlugin::getGraspCandidateObserver() + { + return _graspCandidateObserver; + } + + + } +} + +namespace armarx +{ + + GraspCandidateObserverComponentPluginUser::GraspCandidateObserverComponentPluginUser() + { + addPlugin(plugin); + } + + grasping::GraspCandidateObserverInterfacePrx GraspCandidateObserverComponentPluginUser::getGraspCandidateObserver() + { + return plugin->getGraspCandidateObserver(); + } + + +} + + diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h new file mode 100644 index 0000000000000000000000000000000000000000..e15688a68568b1591ff5aeedfbb6d20337b002a2 --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h @@ -0,0 +1,58 @@ +#pragma once + +#include <ArmarXCore/core/Component.h> +#include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h> + + +namespace armarx +{ + namespace plugins + { + + class GraspCandidateObserverComponentPlugin : public ComponentPlugin + { + public: + using ComponentPlugin::ComponentPlugin; + + void preOnInitComponent() override; + + void preOnConnectComponent() override; + + void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override; + + grasping::GraspCandidateObserverInterfacePrx getGraspCandidateObserver(); + + private: + static constexpr const char* PROPERTY_NAME = "GraspCandidateObserverName"; + grasping::GraspCandidateObserverInterfacePrx _graspCandidateObserver; + }; + + } +} + +namespace armarx +{ + /** + * @brief Provides a ready-to-use GraspCandidateObserver. + */ + class GraspCandidateObserverComponentPluginUser : virtual public ManagedIceObject + { + public: + GraspCandidateObserverComponentPluginUser(); + grasping::GraspCandidateObserverInterfacePrx getGraspCandidateObserver(); + + private: + armarx::plugins::GraspCandidateObserverComponentPlugin* plugin = nullptr; + }; + +} + + +namespace armarx +{ + namespace plugins + { + // Legacy typedef. + using GraspCandidateObserverComponentPluginUser = armarx::GraspCandidateObserverComponentPluginUser; + } +} diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ba8ba16d24d40bbb2312690ef87b4dd9f5619cd5 --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp @@ -0,0 +1,56 @@ +#include "KinematicUnitComponentPlugin.h" + +namespace armarx +{ + namespace plugins + { + + void KinematicUnitComponentPlugin::preOnInitComponent() + { + parent<Component>().usingProxyFromProperty(PROPERTY_NAME); + } + + void KinematicUnitComponentPlugin::preOnConnectComponent() + { + parent<Component>().getProxyFromProperty(_kinematicUnit, PROPERTY_NAME); + } + + void KinematicUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties) + { + if (!properties->hasDefinition(PROPERTY_NAME)) + { + properties->defineRequiredProperty<std::string>( + PROPERTY_NAME, + "Name of the KinematicUnit"); + } + } + + KinematicUnitInterfacePrx KinematicUnitComponentPlugin::getKinematicUnit() + { + return _kinematicUnit; + } + + + } + +} + +namespace armarx +{ + + KinematicUnitComponentPluginUser::KinematicUnitComponentPluginUser() + { + addPlugin(plugin); + } + + KinematicUnitInterfacePrx KinematicUnitComponentPluginUser::getKinematicUnit() + { + return plugin->getKinematicUnit(); + } + + + + +} + + diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h new file mode 100644 index 0000000000000000000000000000000000000000..e3b4062b59500f26314a6ccdb6e5480dd7d34a2b --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h @@ -0,0 +1,59 @@ +#pragma once + +#include <ArmarXCore/core/Component.h> +#include <RobotAPI/interface/units/KinematicUnitInterface.h> + + +namespace armarx +{ + namespace plugins + { + + class KinematicUnitComponentPlugin : public ComponentPlugin + { + public: + using ComponentPlugin::ComponentPlugin; + + void preOnInitComponent() override; + + void preOnConnectComponent() override; + + void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override; + + KinematicUnitInterfacePrx getKinematicUnit(); + + private: + static constexpr const char* PROPERTY_NAME = "KinematicUnitName"; + KinematicUnitInterfacePrx _kinematicUnit; + }; + + } +} + +namespace armarx +{ + /** + * @brief Provides a ready-to-use KinematicUnit. + */ + class KinematicUnitComponentPluginUser : virtual public ManagedIceObject + { + public: + KinematicUnitComponentPluginUser(); + KinematicUnitInterfacePrx getKinematicUnit(); + + private: + armarx::plugins::KinematicUnitComponentPlugin* plugin = nullptr; + }; + + +} + + +namespace armarx +{ + namespace plugins + { + // Legacy typedef. + using KinematicUnitComponentPluginUser = armarx::KinematicUnitComponentPluginUser; + } +} diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp new file mode 100644 index 0000000000000000000000000000000000000000..008d497652fbd0888ef82eb1e5b99650bda11a25 --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp @@ -0,0 +1,54 @@ +#include "NaturalIKComponentPlugin.h" + +namespace armarx +{ + namespace plugins + { + + void NaturalIKComponentPlugin::preOnInitComponent() + { + parent<Component>().usingProxyFromProperty(PROPERTY_NAME); + } + + void NaturalIKComponentPlugin::preOnConnectComponent() + { + parent<Component>().getProxyFromProperty(_naturalIK, PROPERTY_NAME); + } + + void NaturalIKComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties) + { + if (!properties->hasDefinition(PROPERTY_NAME)) + { + properties->defineOptionalProperty<std::string>( + PROPERTY_NAME, + "NaturalIK", + "Name of the NaturalIK"); + } + } + + NaturalIKInterfacePrx NaturalIKComponentPlugin::getNaturalIK() + { + return _naturalIK; + } + + + } +} + +namespace armarx +{ + + NaturalIKComponentPluginUser::NaturalIKComponentPluginUser() + { + addPlugin(plugin); + } + + NaturalIKInterfacePrx NaturalIKComponentPluginUser::getNaturalIK() + { + return plugin->getNaturalIK(); + } + + +} + + diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h new file mode 100644 index 0000000000000000000000000000000000000000..0532416d50ae164baf5645d0cbc30ff50f3e9f48 --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h @@ -0,0 +1,58 @@ +#pragma once + +#include <ArmarXCore/core/Component.h> +#include <RobotAPI/interface/components/NaturalIKInterface.h> + + +namespace armarx +{ + namespace plugins + { + + class NaturalIKComponentPlugin : public ComponentPlugin + { + public: + using ComponentPlugin::ComponentPlugin; + + void preOnInitComponent() override; + + void preOnConnectComponent() override; + + void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override; + + NaturalIKInterfacePrx getNaturalIK(); + + private: + static constexpr const char* PROPERTY_NAME = "NaturalIKName"; + NaturalIKInterfacePrx _naturalIK; + }; + + } +} + +namespace armarx +{ + /** + * @brief Provides a ready-to-use NaturalIK. + */ + class NaturalIKComponentPluginUser : virtual public ManagedIceObject + { + public: + NaturalIKComponentPluginUser(); + NaturalIKInterfacePrx getNaturalIK(); + + private: + armarx::plugins::NaturalIKComponentPlugin* plugin = nullptr; + }; + +} + + +namespace armarx +{ + namespace plugins + { + // Legacy typedef. + using NaturalIKComponentPluginUser = armarx::NaturalIKComponentPluginUser; + } +} diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f40b6cd34a5f5a8f028b714cc7a1517d590f2b3c --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp @@ -0,0 +1,53 @@ +#include "PlatformUnitComponentPlugin.h" + +namespace armarx +{ + namespace plugins + { + + void PlatformUnitComponentPlugin::preOnInitComponent() + { + parent<Component>().usingProxyFromProperty(PROPERTY_NAME); + } + + void PlatformUnitComponentPlugin::preOnConnectComponent() + { + parent<Component>().getProxyFromProperty(_platformUnit, PROPERTY_NAME); + } + + void PlatformUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties) + { + if (!properties->hasDefinition(PROPERTY_NAME)) + { + properties->defineRequiredProperty<std::string>( + PROPERTY_NAME, + "Name of the PlatformUnit"); + } + } + + PlatformUnitInterfacePrx PlatformUnitComponentPlugin::getPlatformUnit() + { + return _platformUnit; + } + + + } +} + +namespace armarx +{ + + PlatformUnitComponentPluginUser::PlatformUnitComponentPluginUser() + { + addPlugin(plugin); + } + + PlatformUnitInterfacePrx PlatformUnitComponentPluginUser::getPlatformUnit() + { + return plugin->getPlatformUnit(); + } + + +} + + diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h new file mode 100644 index 0000000000000000000000000000000000000000..d2cec0f45b698f813d12a10920a122efbb63d00b --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h @@ -0,0 +1,58 @@ +#pragma once + +#include <ArmarXCore/core/Component.h> +#include <RobotAPI/interface/units/PlatformUnitInterface.h> + + +namespace armarx +{ + namespace plugins + { + + class PlatformUnitComponentPlugin : public ComponentPlugin + { + public: + using ComponentPlugin::ComponentPlugin; + + void preOnInitComponent() override; + + void preOnConnectComponent() override; + + void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override; + + PlatformUnitInterfacePrx getPlatformUnit(); + + private: + static constexpr const char* PROPERTY_NAME = "PlatformUnitName"; + PlatformUnitInterfacePrx _platformUnit; + }; + + } +} + +namespace armarx +{ + /** + * @brief Provides a ready-to-use PlatformUnit. + */ + class PlatformUnitComponentPluginUser : virtual public ManagedIceObject + { + public: + PlatformUnitComponentPluginUser(); + PlatformUnitInterfacePrx getPlatformUnit(); + + private: + armarx::plugins::PlatformUnitComponentPlugin* plugin = nullptr; + }; + +} + + +namespace armarx +{ + namespace plugins + { + // Legacy typedef. + using PlatformUnitComponentPluginUser = armarx::PlatformUnitComponentPluginUser; + } +} diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp index e41d1f2aca5bfec043059be72033e9f35d5706a6..c5445458598bfcfe4ea427a4f519f6d5baacddaf 100644 --- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp @@ -130,6 +130,11 @@ namespace armarx::plugins } } + RobotStateComponentInterfacePrx RobotStateComponentPlugin::getRobotStateComponent() + { + return _robotStateComponent; + } + bool RobotStateComponentPlugin::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const { return RemoteRobot::synchronizeLocalClone(robot, _robotStateComponent); @@ -300,6 +305,11 @@ namespace armarx getRobotStateComponentPlugin().setRobotRNSAndNode(id, rnsName, nodeName); } + RobotStateComponentInterfacePrx RobotStateComponentPluginUser::getRobotStateComponent() + { + return getRobotStateComponentPlugin().getRobotStateComponent(); + } + bool RobotStateComponentPluginUser::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const { return getRobotStateComponentPlugin().synchronizeLocalClone(robot); diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h index 5dff431bcabf60f407a63f4f9899232b81eddab9..a338957d196d166494ad031ed3280abeef47b899 100644 --- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h @@ -83,6 +83,8 @@ namespace armarx::plugins VirtualRobot::RobotPtr getRobot(const std::string& id) const; RobotData getRobotData(const std::string& id) const; void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName); + + RobotStateComponentInterfacePrx getRobotStateComponent(); //sync public: bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const; @@ -187,6 +189,9 @@ namespace armarx VirtualRobot::RobotPtr getRobot(const std::string& id) const; RobotStateComponentPlugin::RobotData getRobotData(const std::string& id) const; void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName); + + RobotStateComponentInterfacePrx getRobotStateComponent(); + //sync public: bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const; diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp new file mode 100644 index 0000000000000000000000000000000000000000..383de616a6001cd7358a2fd5604c44f7e669ecc6 --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp @@ -0,0 +1,53 @@ +#include "RobotUnitComponentPlugin.h" + +namespace armarx +{ + namespace plugins + { + + void RobotUnitComponentPlugin::preOnInitComponent() + { + parent<Component>().usingProxyFromProperty(PROPERTY_NAME); + } + + void RobotUnitComponentPlugin::preOnConnectComponent() + { + parent<Component>().getProxyFromProperty(_robotUnit, PROPERTY_NAME); + } + + void RobotUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties) + { + if (!properties->hasDefinition(PROPERTY_NAME)) + { + properties->defineRequiredProperty<std::string>( + PROPERTY_NAME, + "Name of the RobotUnit"); + } + } + + RobotUnitInterfacePrx RobotUnitComponentPlugin::getRobotUnit() + { + return _robotUnit; + } + + + } +} + +namespace armarx +{ + + RobotUnitComponentPluginUser::RobotUnitComponentPluginUser() + { + addPlugin(plugin); + } + + RobotUnitInterfacePrx RobotUnitComponentPluginUser::getRobotUnit() + { + return plugin->getRobotUnit(); + } + + +} + + diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h new file mode 100644 index 0000000000000000000000000000000000000000..4dc53dcb9fff70332663f1dd4a0211f3fdfd916d --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h @@ -0,0 +1,58 @@ +#pragma once + +#include <ArmarXCore/core/Component.h> +#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h> + + +namespace armarx +{ + namespace plugins + { + + class RobotUnitComponentPlugin : public ComponentPlugin + { + public: + using ComponentPlugin::ComponentPlugin; + + void preOnInitComponent() override; + + void preOnConnectComponent() override; + + void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override; + + RobotUnitInterfacePrx getRobotUnit(); + + private: + static constexpr const char* PROPERTY_NAME = "RobotUnitName"; + RobotUnitInterfacePrx _robotUnit; + }; + + } +} + +namespace armarx +{ + /** + * @brief Provides a ready-to-use RobotUnit. + */ + class RobotUnitComponentPluginUser : virtual public ManagedIceObject + { + public: + RobotUnitComponentPluginUser(); + RobotUnitInterfacePrx getRobotUnit(); + + private: + armarx::plugins::RobotUnitComponentPlugin* plugin = nullptr; + }; + +} + + +namespace armarx +{ + namespace plugins + { + // Legacy typedef. + using RobotUnitComponentPluginUser = armarx::RobotUnitComponentPluginUser; + } +} diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp new file mode 100644 index 0000000000000000000000000000000000000000..284533cf16e62834d85ff33e73d36b158943acef --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp @@ -0,0 +1,54 @@ +#include "RobotUnitObserverComponentPlugin.h" + +namespace armarx +{ + namespace plugins + { + + void RobotUnitObserverComponentPlugin::preOnInitComponent() + { + parent<Component>().usingProxyFromProperty(PROPERTY_NAME); + } + + void RobotUnitObserverComponentPlugin::preOnConnectComponent() + { + parent<Component>().getProxyFromProperty(_robotUnitObserver, PROPERTY_NAME); + } + + void RobotUnitObserverComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties) + { + if (!properties->hasDefinition(PROPERTY_NAME)) + { + properties->defineOptionalProperty<std::string>( + PROPERTY_NAME, + "RobotUnitObserver", + "Name of the RobotUnitObserver"); + } + } + + ObserverInterfacePrx RobotUnitObserverComponentPlugin::getRobotUnitObserver() + { + return _robotUnitObserver; + } + + + } +} + +namespace armarx +{ + + RobotUnitObserverComponentPluginUser::RobotUnitObserverComponentPluginUser() + { + addPlugin(plugin); + } + + ObserverInterfacePrx RobotUnitObserverComponentPluginUser::getRobotUnitObserver() + { + return plugin->getRobotUnitObserver(); + } + + +} + + diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h new file mode 100644 index 0000000000000000000000000000000000000000..0ebeff910700102871aa6229259e05f2efec866d --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h @@ -0,0 +1,58 @@ +#pragma once + +#include <ArmarXCore/core/Component.h> +#include <ArmarXCore/interface/observers/ObserverInterface.h> + + +namespace armarx +{ + namespace plugins + { + + class RobotUnitObserverComponentPlugin : public ComponentPlugin + { + public: + using ComponentPlugin::ComponentPlugin; + + void preOnInitComponent() override; + + void preOnConnectComponent() override; + + void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override; + + ObserverInterfacePrx getRobotUnitObserver(); + + private: + static constexpr const char* PROPERTY_NAME = "RobotUnitObserverName"; + ObserverInterfacePrx _robotUnitObserver; + }; + + } +} + +namespace armarx +{ + /** + * @brief Provides a ready-to-use RobotUnitObserver. + */ + class RobotUnitObserverComponentPluginUser : virtual public ManagedIceObject + { + public: + RobotUnitObserverComponentPluginUser(); + ObserverInterfacePrx getRobotUnitObserver(); + + private: + armarx::plugins::RobotUnitObserverComponentPlugin* plugin = nullptr; + }; + +} + + +namespace armarx +{ + namespace plugins + { + // Legacy typedef. + using RobotUnitObserverComponentPluginUser = armarx::RobotUnitObserverComponentPluginUser; + } +} diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1de30ae07c9f8218666121c28350f32c1ca1f5dc --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp @@ -0,0 +1,54 @@ +#include "TrajectoryPlayerComponentPlugin.h" + +namespace armarx +{ + namespace plugins + { + + void TrajectoryPlayerComponentPlugin::preOnInitComponent() + { + parent<Component>().usingProxyFromProperty(PROPERTY_NAME); + } + + void TrajectoryPlayerComponentPlugin::preOnConnectComponent() + { + parent<Component>().getProxyFromProperty(_trajectoryPlayer, PROPERTY_NAME); + } + + void TrajectoryPlayerComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties) + { + if (!properties->hasDefinition(PROPERTY_NAME)) + { + properties->defineOptionalProperty<std::string>( + PROPERTY_NAME, + "TrajectoryPlayer", + "Name of the TrajectoryPlayer"); + } + } + + TrajectoryPlayerInterfacePrx TrajectoryPlayerComponentPlugin::getTrajectoryPlayer() + { + return _trajectoryPlayer; + } + + + } +} + +namespace armarx +{ + + TrajectoryPlayerComponentPluginUser::TrajectoryPlayerComponentPluginUser() + { + addPlugin(plugin); + } + + TrajectoryPlayerInterfacePrx TrajectoryPlayerComponentPluginUser::getTrajectoryPlayer() + { + return plugin->getTrajectoryPlayer(); + } + + +} + + diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h new file mode 100644 index 0000000000000000000000000000000000000000..3fc52df857f7a7225eacb25103dbd7fa16de4af5 --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h @@ -0,0 +1,58 @@ +#pragma once + +#include <ArmarXCore/core/Component.h> +#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h> + + +namespace armarx +{ + namespace plugins + { + + class TrajectoryPlayerComponentPlugin : public ComponentPlugin + { + public: + using ComponentPlugin::ComponentPlugin; + + void preOnInitComponent() override; + + void preOnConnectComponent() override; + + void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override; + + TrajectoryPlayerInterfacePrx getTrajectoryPlayer(); + + private: + static constexpr const char* PROPERTY_NAME = "TrajectoryPlayerName"; + TrajectoryPlayerInterfacePrx _trajectoryPlayer; + }; + + } +} + +namespace armarx +{ + /** + * @brief Provides a ready-to-use TrajectoryPlayer. + */ + class TrajectoryPlayerComponentPluginUser : virtual public ManagedIceObject + { + public: + TrajectoryPlayerComponentPluginUser(); + TrajectoryPlayerInterfacePrx getTrajectoryPlayer(); + + private: + armarx::plugins::TrajectoryPlayerComponentPlugin* plugin = nullptr; + }; + +} + + +namespace armarx +{ + namespace plugins + { + // Legacy typedef. + using TrajectoryPlayerComponentPluginUser = armarx::TrajectoryPlayerComponentPluginUser; + } +} diff --git a/source/RobotAPI/libraries/aron/AronNavigator.cpp b/source/RobotAPI/libraries/aron/AronNavigator.cpp index b24b7f8dcf085c74f3767df01ab6ee42abebe99f..9c5f9118e3893d412b98fcef044ea37abb9d795b 100644 --- a/source/RobotAPI/libraries/aron/AronNavigator.cpp +++ b/source/RobotAPI/libraries/aron/AronNavigator.cpp @@ -23,7 +23,7 @@ #include "AronNavigator.h" #include <ArmarXCore/core/exceptions/Exception.h> - +#include <ArmarXCore/core/exceptions/local/ExpressionException.h> namespace armarx { @@ -31,140 +31,66 @@ namespace armarx { template <typename Tin, typename Tout> - Tout cast_and_check(const AronDataPtr& data, const std::string& path, const std::string& type) + Tout cast_and_check(const AronDataPtr& data, const AronPath& path, const std::string& type) { Tin tdata = Tin::dynamicCast(data); if (!tdata) { - throw LocalException("value cannot be cast to ") << type << ". path: " << path; + throw LocalException("value cannot be cast to ") << type << ". path: " << path.toString(); } return tdata->value; } AronNavigator::AronNavigator(const AronDataPtr& data) - : data(data), path(""), key(""), index(-1) + : data(data) { } - AronNavigator::AronNavigator(const AronDataPtr& data, const std::string& path, const std::string& key, int index) - : data(data), path(path), key(key), index(index) + AronNavigator::AronNavigator(const AronDataPtr& data, const AronPath& path) + : data(data), path(path) { } - AronNavigator AronNavigator::atIndex(size_t index) + ListNavigator AronNavigator::asList() { AronListPtr list = AronListPtr::dynamicCast(data); if (!list) { - throw LocalException("value is not a list. path: ") << path; + throw LocalException("value is not a list. path: ") << path.toString(); } - return AronNavigator(list->elements.at(index), path + "/" + std::to_string(index), "", index); + return ListNavigator(list, path); } - AronNavigator AronNavigator::atKey(const std::string& key) + DictNavigator AronNavigator::asDict() { - AronObjectPtr obj = AronObjectPtr::dynamicCast(data); - if (!obj) - { - throw LocalException("value is not an object. path: ") << path; - } - for (const AronKeyValuePair& pair : obj->elements) + AronDictPtr dict = AronDictPtr::dynamicCast(data); + if (!dict) { - if (pair.key == key) - { - return AronNavigator(pair.value, path + "/" + key, key, 0); - } + throw LocalException("value is not a dict. path: ") << path.toString(); } - throw LocalException("key '") << key << "'not found. path: " << path; + return DictNavigator(dict, path); } - std::vector<AronNavigator> AronNavigator::children() - { - AronListPtr list = AronListPtr::dynamicCast(data); - if (list) - { - std::vector<AronNavigator> result; - for (size_t i = 0; i < list->elements.size(); i++) - { - result.emplace_back(AronNavigator(list->elements.at(i), path + "/" + std::to_string(i), "", i)); - } - return result; - } - AronObjectPtr obj = AronObjectPtr::dynamicCast(data); - if (obj) - { - std::vector<AronNavigator> result; - for (const AronKeyValuePair& pair : obj->elements) - { - result.emplace_back(AronNavigator(pair.value, path + "/" + pair.key, pair.key, -1)); - } - return result; - } - throw LocalException("value is not a list nor an object. path: ") << path; - } - - void AronNavigator::append(const AronDataPtr& value) + Eigen::Vector3f AronNavigator::asVector3f() { - AronListPtr list = AronListPtr::dynamicCast(data); - if (!list) - { - throw LocalException("value is not a list. path: ") << path; - } - list->elements.push_back(value); + return Eigen::Vector3f(checkReinterpretNDArray<float>(3)); } - bool AronNavigator::append(const std::string& key, const AronDataPtr& value) + Eigen::Matrix3f AronNavigator::asMatrix3f() { - int index = keyIndex(key); - if (index >= 0) - { - return false; - } - AronObjectPtr obj = AronObjectPtr::dynamicCast(data); - obj->elements.push_back({key, value}); - return true; + return Eigen::Matrix3f(checkReinterpretNDArray<float>(3, 3)); } - void AronNavigator::set(const std::string& key, const AronDataPtr& value) + Eigen::Matrix4f AronNavigator::asMatrix4f() { - int index = keyIndex(key); - AronObjectPtr obj = AronObjectPtr::dynamicCast(data); - obj->elements.at(index).value = value; + return Eigen::Matrix4f(checkReinterpretNDArray<float>(4, 4)); } - bool AronNavigator::hasKey(const std::string& key) - { - return keyIndex(key) >= 0; - } - int AronNavigator::keyIndex(const std::string& key) - { - AronObjectPtr obj = AronObjectPtr::dynamicCast(data); - if (!obj) - { - throw LocalException("value is not an object. path: ") << path; - } - for (size_t i = 0; i < obj->elements.size(); i++) - { - if (obj->elements.at(i).key == key) - { - return i; - } - } - return -1; - } - bool AronNavigator::deleteKey(const std::string& key) - { - int index = keyIndex(key); - if (index < 0) - { - return false; - } - AronObjectPtr obj = AronObjectPtr::dynamicCast(data); - obj->elements.erase(obj->elements.begin() + index); - return true; - } + + + #define HANDLE_TYPE(cppType, upperType) \ cppType AronNavigator::as##upperType() \ @@ -174,7 +100,7 @@ namespace armarx bool AronNavigator::is##upperType() \ { \ return Aron##upperType##Ptr::dynamicCast(data); \ - } \ + } /*\ void AronNavigator::append##upperType(const cppType& value) \ { \ append(new Aron##upperType(value)); \ @@ -182,7 +108,7 @@ namespace armarx bool AronNavigator::append##upperType(const std::string& key, const cppType& value) \ { \ return append(key, new Aron##upperType(value)); \ - } +}*/ HANDLE_TYPE(std::string, String) HANDLE_TYPE(int, Int) @@ -190,15 +116,6 @@ namespace armarx HANDLE_TYPE(float, Float) HANDLE_TYPE(double, Double) HANDLE_TYPE(bool, Bool) - HANDLE_TYPE(AronBlobValue, Blob) - - HANDLE_TYPE(Eigen::Vector2f, Vector2f) - HANDLE_TYPE(Eigen::Vector3f, Vector3f) - HANDLE_TYPE(Eigen::Vector6f, Vector6f) - HANDLE_TYPE(Eigen::VectorXf, VectorXf) - HANDLE_TYPE(Eigen::Matrix3f, Matrix3f) - HANDLE_TYPE(Eigen::Matrix4f, Matrix4f) - HANDLE_TYPE(Eigen::MatrixXf, MatrixXf) #undef HANDLE_TYPE @@ -209,31 +126,7 @@ namespace armarx return AronNullPtr::dynamicCast(data); } - bool AronNavigator::appendVec(const std::string& key, const std::vector<float>& vec) - { - AronListPtr list = new AronList(); - for (float f : vec) - { - list->elements.push_back(new AronFloat(f)); - } - return append(key, list); - } - std::vector<float> armarx::aron::AronNavigator::AronNavigator::asVecFloat() - { - AronListPtr list = AronListPtr::dynamicCast(data); - if (!list) - { - throw LocalException("value is not a list. path: ") << path; - } - std::vector<float> result; - for (size_t index = 0; index < list->elements.size(); index++) - { - float f = AronNavigator(list->elements.at(index), path + "/" + std::to_string(index), "", index).asFloat(); - result.push_back(f); - } - return result; - } void AronNavigator::writeXml(RapidXmlWriterNode parent) { @@ -285,14 +178,211 @@ namespace armarx }*/ } + + + AronNDArrayPtr AronNavigator::checkCastToNDArray(size_t size) + { + AronNDArrayPtr arr = AronNDArrayPtr::dynamicCast(data); + if (!arr) + { + throw LocalException("value is not an ndarray. path: ") << path.toString(); + } + if (arr->itemSize != (int)size) + { + throw LocalException("item size mismatch for ndarray. itemSize: ") << arr->itemSize << ", requested: " << size << ", path: " << path.toString(); + } + return arr; + } + AronNDArrayPtr AronNavigator::checkCastToNDArray(size_t size, size_t dim1) + { + AronNDArrayPtr arr = checkCastToNDArray(size); + ARMARX_CHECK_W_HINT(arr->dimensions.size() == 1, path.toString()); + ARMARX_CHECK_W_HINT(arr->dimensions.at(0) == (int)dim1, path.toString()); + return arr; + } + + AronNDArrayPtr AronNavigator::checkCastToNDArray(size_t size, size_t dim1, size_t dim2) + { + AronNDArrayPtr arr = checkCastToNDArray(size); + ARMARX_CHECK_W_HINT(arr->dimensions.size() == 2, path.toString()); + ARMARX_CHECK_W_HINT(arr->dimensions.at(0) == (int)dim1, path.toString()); + ARMARX_CHECK_W_HINT(arr->dimensions.at(1) == (int)dim2, path.toString()); + return arr; + } + bool AronNavigator::isList() { return AronListPtr::dynamicCast(data); } - bool AronNavigator::isObject() + bool AronNavigator::isDict() + { + return AronDictPtr::dynamicCast(data); + } + + ListNavigator::ListNavigator(const AronListPtr& list, const AronPath& path) + : list(list), path(path) + { + } + + AronNavigator ListNavigator::at(size_t index) { - return AronObjectPtr::dynamicCast(data); + return AronNavigator(list->elements.at(index), path.appendIndex(index)); } + std::vector<AronNavigator> ListNavigator::children() + { + std::vector<AronNavigator> result; + for (size_t i = 0; i < list->elements.size(); i++) + { + result.emplace_back(AronNavigator(list->elements.at(i), path.appendIndex(i))); + } + return result; + } + + void ListNavigator::append(const AronDataPtr& value) + { + list->elements.push_back(value); + } + + std::vector<AronDataPtr>& ListNavigator::get() + { + return list->elements; + } + + + + std::vector<float> ListNavigator::asVecFloat() + { + std::vector<float> result; + for (size_t index = 0; index < list->elements.size(); index++) + { + float f = AronNavigator(list->elements.at(index), path.appendIndex(index)).asFloat(); + result.push_back(f); + } + return result; + } + + AronPath AronPath::appendIndex(size_t index) const + { + AronPath p; + p.path = path; + p.path.emplace_back(AronPathItem(index)); + return p; + } + + AronPath AronPath::appendKey(const std::string& key) const + { + AronPath p; + p.path = path; + p.path.emplace_back(AronPathItem(key)); + return p; + } + + std::string AronPath::toString() const + { + std::stringstream ss; + bool first = true; + for (const AronPathItem& i : path) + { + if (!first) + { + ss << "/"; + } + first = false; + if (i.index >= 0) + { + ss << i.index; + } + else + { + ss << i.key; + } + } + return ss.str(); + } + + DictNavigator::DictNavigator(const AronDictPtr& dict, const AronPath& path) + : dict(dict), path(path) + { + } + + AronNavigator DictNavigator::at(const std::string& key) + { + for (const AronKeyValuePair& pair : dict->elements) + { + if (pair.key == key) + { + return AronNavigator(pair.value, path.appendKey(key)); + } + } + throw LocalException("key '") << key << "'not found. path: " << path.toString(); + } + + std::vector<AronNavigator> DictNavigator::children() + { + std::vector<AronNavigator> result; + for (const AronKeyValuePair& pair : dict->elements) + { + result.emplace_back(AronNavigator(pair.value, path.appendKey(pair.key))); + } + return result; + } + + bool DictNavigator::append(const std::string& key, const AronDataPtr& value) + { + int index = indexOf(key); + if (index >= 0) + { + return false; + } + dict->elements.push_back({key, value}); + return true; + } + + bool DictNavigator::appendVec(const std::string& key, const std::vector<float>& vec) + { + AronListPtr list = new AronList(); + for (float f : vec) + { + list->elements.push_back(new AronFloat(f)); + } + return append(key, list); + } + + void DictNavigator::set(const std::string& key, const AronDataPtr& value) + { + int index = indexOf(key); + dict->elements.at(index).value = value; + } + + bool DictNavigator::has(const std::string& key) + { + return indexOf(key) >= 0; + } + + int DictNavigator::indexOf(const std::string& key) + { + for (size_t i = 0; i < dict->elements.size(); i++) + { + if (dict->elements.at(i).key == key) + { + return i; + } + } + return -1; + } + + bool DictNavigator::remove(const std::string& key) + { + int index = indexOf(key); + if (index < 0) + { + return false; + } + dict->elements.erase(dict->elements.begin() + index); + return true; + } + + } } diff --git a/source/RobotAPI/libraries/aron/AronNavigator.h b/source/RobotAPI/libraries/aron/AronNavigator.h index ae39728626f92f277c908c7b5bf8d176c7e7eb83..9c568530d13e22dc3f663beaa21e98fd5184481b 100644 --- a/source/RobotAPI/libraries/aron/AronNavigator.h +++ b/source/RobotAPI/libraries/aron/AronNavigator.h @@ -37,30 +37,54 @@ namespace armarx enum { value = false }; }; + struct AronPathItem + { + AronPathItem(int index): index(index), key("") {} + AronPathItem(const std::string& key): index(-1), key(key) {} + int index = -1; + std::string key = ""; + }; + struct AronPath + { + std::vector<AronPathItem> path; + AronPath appendIndex(size_t index) const; + AronPath appendKey(const std::string& key) const; + std::string toString() const; + }; + class ListNavigator; + class DictNavigator; + template <typename T> + class NDArrayNavigator; + class AronNavigator { public: AronNavigator(const AronDataPtr& data); - AronNavigator(const AronDataPtr& data, const std::string& path, const std::string& key, int index); + AronNavigator(const AronDataPtr& data, const AronPath& path); - AronNavigator atIndex(size_t index); - AronNavigator atKey(const std::string& key); - std::vector<AronNavigator> children(); - void append(const AronDataPtr& value); - bool append(const std::string& key, const AronDataPtr& value); - void set(const std::string& key, const AronDataPtr& value); - bool hasKey(const std::string& key); - int keyIndex(const std::string& key); - bool deleteKey(const std::string& key); + ListNavigator asList(); + DictNavigator asDict(); + template <typename T> + NDArrayNavigator<T> asNDArray() + { + AronNDArrayPtr arr = checkCastToNDArray(sizeof(T)); + return NDArrayNavigator<T>(arr, path); + } + + Eigen::Vector3f asVector3f(); + Eigen::Matrix3f asMatrix3f(); + Eigen::Matrix4f asMatrix4f(); bool isList(); - bool isObject(); + bool isDict(); bool isNull(); + + #define HANDLE_TYPE(cppType, upperType) \ cppType as##upperType(); \ bool is##upperType(); \ @@ -73,15 +97,6 @@ namespace armarx HANDLE_TYPE(float, Float) HANDLE_TYPE(double, Double) HANDLE_TYPE(bool, Bool) - HANDLE_TYPE(AronBlobValue, Blob) - - HANDLE_TYPE(Eigen::Vector2f, Vector2f) - HANDLE_TYPE(Eigen::Vector3f, Vector3f) - HANDLE_TYPE(Eigen::Vector6f, Vector6f) - HANDLE_TYPE(Eigen::VectorXf, VectorXf) - HANDLE_TYPE(Eigen::Matrix3f, Matrix3f) - HANDLE_TYPE(Eigen::Matrix4f, Matrix4f) - HANDLE_TYPE(Eigen::MatrixXf, MatrixXf) #undef HANDLE_TYPE @@ -91,21 +106,33 @@ namespace armarx template <typename T> std::vector<T> asStdVector(); - //TODO - bool appendVec(const std::string& key, const std::vector<float>& vec); - std::vector<float> asVecFloat(); + template <typename T> std::map<std::string, T> asStdMap(); void writeXml(RapidXmlWriterNode parent); + private: + AronNDArrayPtr checkCastToNDArray(size_t size); + AronNDArrayPtr checkCastToNDArray(size_t size, size_t dim1); + AronNDArrayPtr checkCastToNDArray(size_t size, size_t dim1, size_t dim2); + template <typename T> + T* checkReinterpretNDArray(size_t dim1) + { + AronNDArrayPtr arr = checkCastToNDArray(sizeof(T), dim1); + return reinterpret_cast<T*>(arr->data.data()); + } + template <typename T> + T* checkReinterpretNDArray(size_t dim1, size_t dim2) + { + AronNDArrayPtr arr = checkCastToNDArray(sizeof(T), dim1, dim2); + return reinterpret_cast<T*>(arr->data.data()); + } private: AronDataPtr data; - std::string path; - std::string key; - int index; + AronPath path; }; template<typename T> @@ -126,18 +153,84 @@ namespace armarx HANDLE_TYPE(float, Float) HANDLE_TYPE(double, Double) HANDLE_TYPE(bool, Bool) - HANDLE_TYPE(AronBlobValue, Blob) - - HANDLE_TYPE(Eigen::Vector2f, Vector2f) - HANDLE_TYPE(Eigen::Vector3f, Vector3f) - HANDLE_TYPE(Eigen::Vector6f, Vector6f) - HANDLE_TYPE(Eigen::VectorXf, VectorXf) - HANDLE_TYPE(Eigen::Matrix3f, Matrix3f) - HANDLE_TYPE(Eigen::Matrix4f, Matrix4f) - HANDLE_TYPE(Eigen::MatrixXf, MatrixXf) #undef HANDLE_TYPE + class ListNavigator + { + public: + ListNavigator(const AronListPtr& list, const AronPath& path); + AronNavigator at(size_t index); + std::vector<AronNavigator> children(); + void append(const AronDataPtr& value); + std::vector<AronDataPtr>& get(); + + //TODO + std::vector<float> asVecFloat(); + + private: + AronListPtr list; + AronPath path; + }; + + class DictNavigator + { + public: + DictNavigator(const AronDictPtr& dict, const AronPath& path); + AronNavigator at(const std::string& key); + std::vector<AronNavigator> children(); + bool append(const std::string& key, const AronDataPtr& value); + bool appendVec(const std::string& key, const std::vector<float>& vec); + void set(const std::string& key, const AronDataPtr& value); + bool has(const std::string& key); + int indexOf(const std::string& key); + bool remove(const std::string& key); + private: + AronDictPtr dict; + AronPath path; + }; + + template <typename T> + class NDArrayNavigator + { + public: + NDArrayNavigator(const AronNDArrayPtr& arr, const AronPath& path) + : arr(arr), path(path) {} + size_t dimensions() + { + return arr->dimensions.size(); + } + int size(size_t dim) + { + return arr->dimensions.at(dim); + } + + T& at(size_t i) + { + ARMARX_CHECK(dimensions() == 1); + return reinterpret_cast<T&>(_at(i)); + } + T& at(size_t i, size_t j) + { + ARMARX_CHECK(dimensions() == 2); + return reinterpret_cast<T&>(_at(i * arr->dimensions.at(1) + j)); + } + T* data() + { + return reinterpret_cast<T*>(arr->data.data()); + } + private: + Ice::Byte& _at(size_t i) + { + + return arr->data.at(arr->itemSize * i); + } + + private: + AronNDArrayPtr arr; + AronPath path; + }; + } } diff --git a/source/RobotAPI/libraries/aron/types/TypesMap.cpp b/source/RobotAPI/libraries/aron/types/TypesMap.cpp index 91b29d5b8d8a7723856ec1017160aeae57b493f6..2b3ca35316b4be12d4242d5a8168f931522dac1f 100644 --- a/source/RobotAPI/libraries/aron/types/TypesMap.cpp +++ b/source/RobotAPI/libraries/aron/types/TypesMap.cpp @@ -37,7 +37,7 @@ std::string TypesMap::getCppType(const types::AbstractTypePtr& type) { SingleTypePtr single = SingleTypePtr::dynamicCast(type); ListTypePtr list = ListTypePtr::dynamicCast(type); - ObjectTypePtr obj = ObjectTypePtr::dynamicCast(type); + DictTypePtr dict = DictTypePtr::dynamicCast(type); if (single) { return getCppType(single->namespaces, single->typeName); @@ -46,9 +46,9 @@ std::string TypesMap::getCppType(const types::AbstractTypePtr& type) { return "std::vector<" + getCppType(list->childtype) + ">"; } - if (obj) + if (dict) { - return "std::map<std::string, " + getCppType(obj->childtype) + ">"; + return "std::map<std::string, " + getCppType(dict->childtype) + ">"; } throw LocalException("Unsupported type"); diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp index cea227464abe29898488605e6dcb38f5e50fb4a5..90c68c71c1fd0ca963f8f4eb1bc05c1d926be1f7 100644 --- a/source/RobotAPI/libraries/core/Trajectory.cpp +++ b/source/RobotAPI/libraries/core/Trajectory.cpp @@ -1971,6 +1971,36 @@ namespace armarx return getDeriv(dim, 0); } + Eigen::VectorXf Trajectory::TrajData::getPositionsAsVectorXf() const + { + if (!trajectory) + { + throw LocalException("Ptr to trajectory is NULL"); + } + size_t numDim = trajectory->dim(); + Eigen::VectorXf result(numDim); + for (std::size_t i = 0; i < numDim; ++i) + { + result(i) = getPosition(i); + } + return result; + } + + Eigen::VectorXd Trajectory::TrajData::getPositionsAsVectorXd() const + { + if (!trajectory) + { + throw LocalException("Ptr to trajectory is NULL"); + } + size_t numDim = trajectory->dim(); + Eigen::VectorXd result(numDim); + for (std::size_t i = 0; i < numDim; ++i) + { + result(i) = getPosition(i); + } + return result; + } + double Trajectory::TrajData::getDeriv(size_t dim, size_t derivation) const { if (!trajectory) diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h index 2feaea611590e10f71b227bcb845c46dd37f59ce..990de647c0bb74092d96a7774830cd1fb6f96f98 100644 --- a/source/RobotAPI/libraries/core/Trajectory.h +++ b/source/RobotAPI/libraries/core/Trajectory.h @@ -128,6 +128,9 @@ namespace armarx { return getPositionsAs<double>(); } + Eigen::VectorXf getPositionsAsVectorXf() const; + Eigen::VectorXd getPositionsAsVectorXd() const; + template<class T> void writePositionsToNameValueMap(std::map<std::string, T>& map) const diff --git a/source/RobotAPI/libraries/diffik/CMakeLists.txt b/source/RobotAPI/libraries/diffik/CMakeLists.txt index 9699d191373f2210ec6677e05a8af1b6315ca71d..05a777679813c9f2411dbe6cce56141ab75dae77 100644 --- a/source/RobotAPI/libraries/diffik/CMakeLists.txt +++ b/source/RobotAPI/libraries/diffik/CMakeLists.txt @@ -6,15 +6,22 @@ armarx_set_target("Library: ${LIB_NAME}") set(LIBS ArmarXCore RobotAPICore + StructuralJson + SimpleJsonLogger ) set(LIB_FILES NaturalDiffIK.cpp SimpleDiffIK.cpp + RobotPlacement.cpp + GraspTrajectory.cpp ) set(LIB_HEADERS NaturalDiffIK.h SimpleDiffIK.h + DiffIKProvider.h + RobotPlacement.h + GraspTrajectory.h ) diff --git a/source/RobotAPI/libraries/diffik/DiffIKProvider.h b/source/RobotAPI/libraries/diffik/DiffIKProvider.h new file mode 100644 index 0000000000000000000000000000000000000000..a75434af401538fc92697010ee6fc3d6b43e9071 --- /dev/null +++ b/source/RobotAPI/libraries/diffik/DiffIKProvider.h @@ -0,0 +1,47 @@ +/* + * This file is part of ArmarX. + * + * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), + * Karlsruhe Institute of Technology (KIT), all rights reserved. + * + * ArmarX is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * ArmarX is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu) + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#pragma once + +#include <boost/shared_ptr.hpp> +#include <Eigen/Dense> + +namespace armarx +{ + typedef boost::shared_ptr<class DiffIKProvider> DiffIKProviderPtr; + + struct DiffIKResult + { + bool reachable; + float posError; + float oriError; + Eigen::VectorXf jointValues; + + }; + class DiffIKProvider + { + public: + virtual DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose) = 0; + virtual DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues) = 0; + }; +} diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6d76464e94326b8e864ace29ec294d10aff65402 --- /dev/null +++ b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp @@ -0,0 +1,461 @@ +/* + * This file is part of ArmarX. + * + * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), + * Karlsruhe Institute of Technology (KIT), all rights reserved. + * + * ArmarX is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * ArmarX is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu) + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#include "GraspTrajectory.h" + +#include <ArmarXCore/core/exceptions/local/ExpressionException.h> + +using namespace armarx; + + +GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget) + : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(0), + feedForwardPosVelocity(0, 0, 0), feedForwardOriVelocity(0, 0, 0), + feedForwardHandJointsVelocity(Eigen::VectorXf::Zero(handJointsTarget.rows())) +{ } + +GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, + float dt, const Eigen::Vector3f& feedForwardPosVelocity, const Eigen::Vector3f& feedForwardOriVelocity, + const Eigen::VectorXf& feedForwardHandJointsVelocity) + : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(dt), + feedForwardPosVelocity(feedForwardPosVelocity), feedForwardOriVelocity(feedForwardOriVelocity), + feedForwardHandJointsVelocity(feedForwardHandJointsVelocity) +{ } + +Eigen::Vector3f GraspTrajectory::Keypoint::getTargetPosition() const +{ + return ::math::Helpers::GetPosition(tcpTarget); +} + +Eigen::Matrix3f GraspTrajectory::Keypoint::getTargetOrientation() const +{ + return ::math::Helpers::GetOrientation(tcpTarget); +} + +Eigen::Matrix4f GraspTrajectory::Keypoint::getTargetPose() const +{ + return tcpTarget; +} + +void GraspTrajectory::Keypoint::updateVelocities(const GraspTrajectory::KeypointPtr& prev, float dt) +{ + Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(prev->tcpTarget); + Eigen::Matrix3f ori0 = ::math::Helpers::GetOrientation(prev->tcpTarget); + Eigen::VectorXf hnd0 = prev->handJointsTarget; + + Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(tcpTarget); + Eigen::Matrix3f ori1 = ::math::Helpers::GetOrientation(tcpTarget); + Eigen::VectorXf hnd1 = handJointsTarget; + + Eigen::Vector3f dpos = pos1 - pos0; + Eigen::Vector3f dori = ::math::Helpers::GetRotationVector(ori0, ori1); + Eigen::VectorXf dhnd = hnd1 - hnd0; + + this->dt = dt; + feedForwardPosVelocity = dpos / dt; + feedForwardOriVelocity = dori / dt; + feedForwardHandJointsVelocity = dhnd / dt; +} + +GraspTrajectory::GraspTrajectory(const Eigen::Matrix4f& tcpStart, const Eigen::VectorXf& handJointsStart) +{ + KeypointPtr keypoint(new Keypoint(tcpStart, handJointsStart)); + keypointMap[0] = keypoints.size(); + keypoints.push_back(keypoint); +} + +void GraspTrajectory::addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt) +{ + ARMARX_CHECK_EQUAL(GetHandJointCount(), handJointsTarget.rows()); + KeypointPtr prev = lastKeypoint(); + KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget)); + keypoint->updateVelocities(prev, dt); + float t = getDuration() + dt; + keypointMap[t] = keypoints.size(); + keypoints.push_back(keypoint); +} + +size_t GraspTrajectory::getKeypointCount() const +{ + return keypoints.size(); +} + +void GraspTrajectory::insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt) +{ + ARMARX_CHECK_EQUAL(GetHandJointCount(), handJointsTarget.rows()); + if (index <= 0 || index > keypoints.size()) + { + throw LocalException("Index out of range" + std::to_string(index)); + } + KeypointPtr prev = keypoints.at(index - 1); + KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget)); + keypoint->updateVelocities(prev, dt); + if (index < keypoints.size()) + { + KeypointPtr next = keypoints.at(index); + next->updateVelocities(keypoint, next->dt); + } + keypoints.insert(keypoints.begin() + index, keypoint); + updateKeypointMap(); +} + +void GraspTrajectory::removeKeypoint(size_t index) +{ + if (index <= 0 || index >= keypoints.size()) + { + throw LocalException("Index out of range" + std::to_string(index)); + } + keypoints.erase(keypoints.begin() + index); + if (index < keypoints.size()) + { + KeypointPtr prev = keypoints.at(index - 1); + KeypointPtr next = keypoints.at(index); + next->updateVelocities(prev, next->dt); + } + updateKeypointMap(); +} + +void GraspTrajectory::replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt) +{ + ARMARX_CHECK_EQUAL(GetHandJointCount(), handJointsTarget.rows()); + if (index <= 0 || index >= keypoints.size()) + { + throw LocalException("Index out of range" + std::to_string(index)); + } + KeypointPtr prev = keypoints.at(index - 1); + KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget)); + keypoint->updateVelocities(prev, dt); + keypoints.at(index) = keypoint; + updateKeypointMap(); +} + +void GraspTrajectory::setKeypointDt(size_t index, float dt) +{ + if (index <= 0 || index >= keypoints.size()) + { + throw LocalException("Index out of range" + std::to_string(index)); + } + KeypointPtr prev = keypoints.at(index - 1); + KeypointPtr& keypoint = keypoints.at(index); + keypoint->updateVelocities(prev, dt); + updateKeypointMap(); +} + +GraspTrajectory::KeypointPtr& GraspTrajectory::lastKeypoint() +{ + return keypoints.at(keypoints.size() - 1); +} + +GraspTrajectory::KeypointPtr& GraspTrajectory::getKeypoint(int i) +{ + return keypoints.at(i); +} + +Eigen::Matrix4f GraspTrajectory::getStartPose() +{ + return getKeypoint(0)->getTargetPose(); +} + +void GraspTrajectory::getIndex(float t, int& i1, int& i2, float& f) +{ + if (t <= 0) + { + i1 = 0; + i2 = 0; + f = 0; + return; + } + std::map<float, size_t>::const_iterator it, prev; + it = keypointMap.upper_bound(t); + if (it == keypointMap.end()) + { + i1 = keypoints.size() - 1; + i2 = keypoints.size() - 1; + f = 0; + return; + } + prev = std::prev(it); + i1 = prev->second; + i2 = it->second; + f = ::math::Helpers::ILerp(prev->first, it->first, t); +} + +Eigen::Vector3f GraspTrajectory::GetPosition(float t) +{ + int i1, i2; + float f; + getIndex(t, i1, i2, f); + return ::math::Helpers::Lerp(getKeypoint(i1)->getTargetPosition(), getKeypoint(i2)->getTargetPosition(), f); +} + +Eigen::Matrix3f GraspTrajectory::GetOrientation(float t) +{ + int i1, i2; + float f; + getIndex(t, i1, i2, f); + Eigen::Vector3f oriVel = GetOrientationDerivative(t); + if (oriVel.squaredNorm() == 0) + { + return getKeypoint(i1)->getTargetOrientation(); + } + Eigen::AngleAxisf aa(oriVel.norm(), oriVel.normalized()); + aa.angle() = aa.angle() * f * getKeypoint(i2)->dt; + return aa.toRotationMatrix() * getKeypoint(i1)->getTargetOrientation(); +} + +Eigen::Matrix4f GraspTrajectory::GetPose(float t) +{ + return ::math::Helpers::CreatePose(GetPosition(t), GetOrientation(t)); +} + +std::vector<Eigen::Matrix4f> GraspTrajectory::getAllKeypointPoses() +{ + std::vector<Eigen::Matrix4f> res; + for (const KeypointPtr& keypoint : keypoints) + { + res.emplace_back(keypoint->getTargetPose()); + } + return res; +} + +std::vector<Eigen::Vector3f> GraspTrajectory::getAllKeypointPositions() +{ + std::vector<Eigen::Vector3f> res; + for (const KeypointPtr& keypoint : keypoints) + { + res.emplace_back(keypoint->getTargetPosition()); + } + return res; +} + +std::vector<Eigen::Matrix3f> GraspTrajectory::getAllKeypointOrientations() +{ + std::vector<Eigen::Matrix3f> res; + for (const KeypointPtr& keypoint : keypoints) + { + res.emplace_back(keypoint->getTargetOrientation()); + } + return res; +} + +Eigen::VectorXf GraspTrajectory::GetHandValues(float t) +{ + int i1, i2; + float f; + getIndex(t, i1, i2, f); + + return getKeypoint(i1)->handJointsTarget * (1 - f) + getKeypoint(i2)->handJointsTarget * f; +} + +Eigen::Vector3f GraspTrajectory::GetPositionDerivative(float t) +{ + int i1, i2; + float f; + getIndex(t, i1, i2, f); + return getKeypoint(i2)->feedForwardPosVelocity; +} + +Eigen::Vector3f GraspTrajectory::GetOrientationDerivative(float t) +{ + int i1, i2; + float f; + getIndex(t, i1, i2, f); + return getKeypoint(i2)->feedForwardOriVelocity; +} + +Eigen::Vector6f GraspTrajectory::GetTcpDerivative(float t) +{ + Eigen::Vector6f ffVel; + ffVel.block<3, 1>(0, 0) = GetPositionDerivative(t); + ffVel.block<3, 1>(3, 0) = GetOrientationDerivative(t); + return ffVel; +} + +Eigen::VectorXf GraspTrajectory::GetHandJointsDerivative(float t) +{ + int i1, i2; + float f; + getIndex(t, i1, i2, f); + return getKeypoint(i2)->feedForwardHandJointsVelocity; +} + +float GraspTrajectory::getDuration() const +{ + return keypointMap.rbegin()->first; +} + +GraspTrajectory::Length GraspTrajectory::calculateLength() const +{ + Length l; + for (size_t i = 1; i < keypoints.size(); i++) + { + KeypointPtr k0 = keypoints.at(i - 1); + KeypointPtr k1 = keypoints.at(i); + + Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(k0->tcpTarget); + Eigen::Matrix3f ori0 = ::math::Helpers::GetOrientation(k0->tcpTarget); + + Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(k1->tcpTarget); + Eigen::Matrix3f ori1 = ::math::Helpers::GetOrientation(k1->tcpTarget); + + l.pos += (pos1 - pos0).norm(); + l.ori += fabs(::math::Helpers::GetAngleAxisFromTo(ori0, ori1).angle()); + } + return l; +} + +int GraspTrajectory::GetHandJointCount() const +{ + return keypoints.at(0)->handJointsTarget.rows(); +} + +GraspTrajectoryPtr GraspTrajectory::getTranslatedAndRotated(const Eigen::Vector3f& translation, const Eigen::Matrix3f& rotation) +{ + GraspTrajectoryPtr traj(new GraspTrajectory(::math::Helpers::TranslateAndRotatePose(getKeypoint(0)->getTargetPose(), translation, rotation), getKeypoint(0)->handJointsTarget)); + for (size_t i = 1; i < keypoints.size(); i++) + { + KeypointPtr& kp = keypoints.at(i); + traj->addKeypoint(::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation), kp->handJointsTarget, kp->dt); + } + return traj; +} + +GraspTrajectoryPtr GraspTrajectory::getTransformed(const Eigen::Matrix4f& transform) +{ + GraspTrajectoryPtr traj(new GraspTrajectory(transform * getStartPose(), getKeypoint(0)->handJointsTarget)); + for (size_t i = 1; i < keypoints.size(); i++) + { + KeypointPtr& kp = keypoints.at(i); + traj->addKeypoint(transform * kp->getTargetPose(), kp->handJointsTarget, kp->dt); + } + return traj; +} + +GraspTrajectoryPtr GraspTrajectory::getClone() +{ + return getTransformed(Eigen::Matrix4f::Identity()); +} + +GraspTrajectoryPtr GraspTrajectory::getTransformedToGraspPose(const Eigen::Matrix4f& target, const Eigen::Vector3f& handForward) +{ + Eigen::Matrix4f startPose = getStartPose(); + Eigen::Vector3f targetHandForward = ::math::Helpers::TransformDirection(target, handForward); + Eigen::Vector3f trajHandForward = ::math::Helpers::TransformDirection(startPose, handForward); + Eigen::Vector3f up(0, 0, 1); + + float angle = ::math::Helpers::Angle(targetHandForward, trajHandForward, up); + Eigen::AngleAxisf aa(angle, up); + + Eigen::Matrix4f transform = ::math::Helpers::CreateTranslationRotationTranslationPose(-::math::Helpers::GetPosition(startPose), aa.toRotationMatrix(), ::math::Helpers::GetPosition(target)); + return getTransformed(transform); +} + +SimpleDiffIK::Reachability GraspTrajectory::calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, SimpleDiffIK::Parameters params) +{ + return SimpleDiffIK::CalculateReachability(getAllKeypointPoses(), Eigen::VectorXf::Zero(rns->getSize()), rns, tcp, params); +} + +void GraspTrajectory::writeToFile(const std::string& filename) +{ + RapidXmlWriter writer; + RapidXmlWriterNode root = writer.createRootNode("GraspTrajectory"); + for (const KeypointPtr& keypoint : keypoints) + { + SimpleJsonLoggerEntry e; + e.Add("dt", keypoint->dt); + e.AddAsArr("Pose", keypoint->tcpTarget); + e.AddAsArr("HandValues", keypoint->handJointsTarget); + root.append_string_node("Keypoint", e.obj->toJsonString(0, "", true)); + } + writer.saveToFile(filename, true); +} + +GraspTrajectoryPtr GraspTrajectory::ReadFromFile(const grasping::GraspCandidatePtr& cnd) +{ + std::string packageName = "Armar6Skills";// cnd->executionHints->graspTrajectoryPackage; + armarx::CMakePackageFinder finder(packageName); + std::string dataDir = finder.getDataDir() + "/" + packageName; + return GraspTrajectory::ReadFromFile(dataDir + "/motions/grasps/" + cnd->executionHints->graspTrajectoryName + ".xml"); +} + +GraspTrajectoryPtr GraspTrajectory::ReadFromReader(const RapidXmlReaderPtr& reader) +{ + RapidXmlReaderNode root = reader->getRoot("GraspTrajectory"); + GraspTrajectoryPtr traj; + for (const RapidXmlReaderNode& kpNode : root.nodes("Keypoint")) + { + StructuralJsonParser p(kpNode.value()); + p.parse(); + JPathNavigator nav(p.parsedJson); + float dt = nav.selectSingleNode("dt").asFloat(); + + Eigen::Matrix4f pose; + std::vector<JPathNavigator> rows = nav.select("Pose/*"); + for (int i = 0; i < 4; i++) + { + std::vector<JPathNavigator> cells = rows.at(i).select("*"); + for (int j = 0; j < 4; j++) + { + pose(i, j) = cells.at(j).asFloat(); + } + } + + Eigen::Vector3f handValues; + std::vector<JPathNavigator> cells = nav.select("HandValues/*"); + for (int j = 0; j < 3; j++) + { + handValues(j) = cells.at(j).asFloat(); + } + + if (!traj) + { + traj = GraspTrajectoryPtr(new GraspTrajectory(pose, handValues)); + } + else + { + traj->addKeypoint(pose, handValues, dt); + } + } + return traj; +} + +GraspTrajectoryPtr GraspTrajectory::ReadFromFile(const std::string& filename) +{ + return ReadFromReader(RapidXmlReader::FromFile(filename)); +} + +GraspTrajectoryPtr GraspTrajectory::ReadFromString(const std::string& xml) +{ + return ReadFromReader(RapidXmlReader::FromXmlString(xml)); +} + +void GraspTrajectory::updateKeypointMap() +{ + keypointMap.clear(); + float t = 0; + for (size_t i = 0; i < keypoints.size(); i++) + { + t += getKeypoint(i)->dt; + keypointMap[t] = i; + } +} diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.h b/source/RobotAPI/libraries/diffik/GraspTrajectory.h new file mode 100644 index 0000000000000000000000000000000000000000..3e5633d59e9a2f2c8ad533735370a1f008694089 --- /dev/null +++ b/source/RobotAPI/libraries/diffik/GraspTrajectory.h @@ -0,0 +1,151 @@ +/* + * This file is part of ArmarX. + * + * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), + * Karlsruhe Institute of Technology (KIT), all rights reserved. + * + * ArmarX is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * ArmarX is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu) + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#pragma once + +#include <boost/shared_ptr.hpp> +#include <Eigen/Dense> +#include <VirtualRobot/math/AbstractFunctionR1R6.h> +#include <VirtualRobot/math/Helpers.h> +#include <map> +#include <ArmarXCore/core/exceptions/Exception.h> +#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h> +#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlWriter.h> +#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h> +#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h> +#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h> +#include <RobotAPI/libraries/diffik/SimpleDiffIK.h> +#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h> +#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h> +#include <ArmarXCore/interface/serialization/Eigen.h> + +namespace armarx +{ + typedef boost::shared_ptr<class GraspTrajectory> GraspTrajectoryPtr; + + class GraspTrajectory + { + public: + class Keypoint; + typedef boost::shared_ptr<Keypoint> KeypointPtr; + + class Keypoint + { + public: + Eigen::Matrix4f tcpTarget; + Eigen::VectorXf handJointsTarget; + float dt; + Eigen::Vector3f feedForwardPosVelocity; + Eigen::Vector3f feedForwardOriVelocity; + Eigen::VectorXf feedForwardHandJointsVelocity; + + Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget); + Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt, + const Eigen::Vector3f& feedForwardPosVelocity, const Eigen::Vector3f& feedForwardOriVelocity, + const Eigen::VectorXf& feedForwardHandJointsVelocity); + Eigen::Vector3f getTargetPosition() const; + Eigen::Matrix3f getTargetOrientation() const; + Eigen::Matrix4f getTargetPose() const; + void updateVelocities(const KeypointPtr& prev, float dt); + }; + + struct Length + { + float pos = 0; + float ori = 0; + }; + + + public: + GraspTrajectory(const Eigen::Matrix4f& tcpStart, const Eigen::VectorXf& handJointsStart); + + void addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt); + + size_t getKeypointCount() const; + + void insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt); + + void removeKeypoint(size_t index); + + void replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt); + + void setKeypointDt(size_t index, float dt); + + KeypointPtr& lastKeypoint(); + KeypointPtr& getKeypoint(int i); + Eigen::Matrix4f getStartPose(); + + void getIndex(float t, int& i1, int& i2, float& f); + + Eigen::Vector3f GetPosition(float t); + + Eigen::Matrix3f GetOrientation(float t); + + Eigen::Matrix4f GetPose(float t); + + std::vector<Eigen::Matrix4f> getAllKeypointPoses(); + std::vector<Eigen::Vector3f> getAllKeypointPositions(); + std::vector<Eigen::Matrix3f> getAllKeypointOrientations(); + + Eigen::VectorXf GetHandValues(float t); + + Eigen::Vector3f GetPositionDerivative(float t); + + Eigen::Vector3f GetOrientationDerivative(float t); + + Eigen::Vector6f GetTcpDerivative(float t); + + Eigen::VectorXf GetHandJointsDerivative(float t); + + float getDuration() const; + + Length calculateLength() const; + int GetHandJointCount() const; + + + GraspTrajectoryPtr getTranslatedAndRotated(const Eigen::Vector3f& translation, const Eigen::Matrix3f& rotation); + GraspTrajectoryPtr getTransformed(const Eigen::Matrix4f& transform); + + GraspTrajectoryPtr getClone(); + + GraspTrajectoryPtr getTransformedToGraspPose(const Eigen::Matrix4f& target, const Eigen::Vector3f& handForward = Eigen::Vector3f::UnitZ()); + + SimpleDiffIK::Reachability calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters()); + + void writeToFile(const std::string& filename); + + static GraspTrajectoryPtr ReadFromFile(const grasping::GraspCandidatePtr& cnd); + + static GraspTrajectoryPtr ReadFromReader(const RapidXmlReaderPtr& reader); + static GraspTrajectoryPtr ReadFromFile(const std::string& filename); + static GraspTrajectoryPtr ReadFromString(const std::string& xml); + + private: + + void updateKeypointMap(); + + private: + std::vector<KeypointPtr> keypoints; + std::map<float, size_t> keypointMap; + }; +} diff --git a/source/RobotAPI/libraries/diffik/RobotPlacement.cpp b/source/RobotAPI/libraries/diffik/RobotPlacement.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4075c9aae309b66ce51d7125ede732b4869ba88d --- /dev/null +++ b/source/RobotAPI/libraries/diffik/RobotPlacement.cpp @@ -0,0 +1,68 @@ +/* + * This file is part of ArmarX. + * + * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), + * Karlsruhe Institute of Technology (KIT), all rights reserved. + * + * ArmarX is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * ArmarX is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu) + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#include "RobotPlacement.h" + +#include <VirtualRobot/math/Helpers.h> + +using namespace armarx; + +RobotPlacement::RobotPlacement(const DiffIKProviderPtr& ikProvider) +{ +} + +std::vector<Eigen::Matrix4f> RobotPlacement::CreateGrid(float dx, int minx, int maxx, float dy, int miny, int maxy, float da, int mina, int maxa) +{ + std::vector<Eigen::Matrix4f> r; + for (int x = minx; x <= maxx; x++) + for (int y = miny; y <= maxy; y++) + for (int a = mina; a <= maxa; a++) + { + r.emplace_back(math::Helpers::CreatePose(Eigen::Vector3f(x * dx, y * dy, 0), Eigen::AngleAxisf(a * da, Eigen::Vector3f::UnitZ()).toRotationMatrix())); + } + return r; +} + +std::vector<RobotPlacement::Result> RobotPlacement::evaluatePlacements(const std::vector<Eigen::Matrix4f>& robotPlacements, const RobotPlacement::PlacementParams& params) +{ + std::vector<RobotPlacement::Result> r; + std::vector<Eigen::Matrix4f> tcpTargets; + for (const Eigen::Matrix4f& pp : params.prePoses) + { + tcpTargets.emplace_back(pp); + } + std::vector<Eigen::Matrix4f> grasPoses = params.graspTrajectory->getAllKeypointPoses(); + + for (const Eigen::Matrix4f& placement : robotPlacements) + { + Eigen::Matrix4f invPlacement = placement.inverse(); + std::vector<Eigen::Matrix4f> localPoses; + for (const Eigen::Matrix4f& tcpPose : tcpTargets) + { + localPoses.emplace_back(invPlacement * tcpPose); + } + DiffIKResult ikResult = ikProvider->SolveAbsolute(localPoses.at(0)); + throw LocalException("not implemented"); + } + return {}; +} diff --git a/source/RobotAPI/libraries/diffik/RobotPlacement.h b/source/RobotAPI/libraries/diffik/RobotPlacement.h new file mode 100644 index 0000000000000000000000000000000000000000..a677a7a1181b1a88dc731da6f4859c188453f5bf --- /dev/null +++ b/source/RobotAPI/libraries/diffik/RobotPlacement.h @@ -0,0 +1,59 @@ +/* + * This file is part of ArmarX. + * + * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), + * Karlsruhe Institute of Technology (KIT), all rights reserved. + * + * ArmarX is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * ArmarX is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu) + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#pragma once + +#include "DiffIKProvider.h" +#include "GraspTrajectory.h" + +#include <boost/shared_ptr.hpp> + +namespace armarx +{ + typedef boost::shared_ptr<class RobotPlacement> RobotPlacementPtr; + + class RobotPlacement + { + public: + struct Result + { + DiffIKResult ikResult; + }; + struct PlacementParams + { + std::vector<Eigen::Matrix4f> prePoses; + GraspTrajectoryPtr graspTrajectory; + }; + public: + RobotPlacement(const DiffIKProviderPtr& ikProvider); + + static std::vector<Eigen::Matrix4f> CreateGrid(float dx, int minx, int maxx, float dy, int miny, int maxy, float da, int mina, int maxa); + + std::vector<Result> evaluatePlacements(const std::vector<Eigen::Matrix4f>& robotPlacements, const PlacementParams& params); + + + private: + DiffIKProviderPtr ikProvider; + bool returnOnlyReachable = true; + }; +} diff --git a/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp index c1a91dacdec922aac74d8160fff0f8c03256353f..c3f1a32dbaa1001d06762522080ef5367c072b70 100644 --- a/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp +++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp @@ -140,4 +140,35 @@ namespace armarx return r; } + SimpleDiffIKProvider::SimpleDiffIKProvider(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, SimpleDiffIK::Parameters params) + : rns(rns), tcp(tcp), params(params) + { + } + + DiffIKResult SimpleDiffIKProvider::SolveAbsolute(const Eigen::Matrix4f& targetPose) + { + params.resetRnsValues = true; + SimpleDiffIK::Result result = SimpleDiffIK::CalculateDiffIK(targetPose, rns, tcp, params); + DiffIKResult r; + r.jointValues = rns->getJointValuesEigen(); + r.oriError = result.oriError; + r.posError = result.posError; + r.reachable = result.reached; + return r; + + } + + DiffIKResult SimpleDiffIKProvider::SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues) + { + params.resetRnsValues = false; + rns->setJointValues(startJointValues); + SimpleDiffIK::Result result = SimpleDiffIK::CalculateDiffIK(targetPose, rns, tcp, params); + DiffIKResult r; + r.jointValues = rns->getJointValuesEigen(); + r.oriError = result.oriError; + r.posError = result.posError; + r.reachable = result.reached; + return r; + } + } diff --git a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h index ffd371a6fe8d8ea8cd080e97fc1658c632b46868..e51be99211a5c8c71f612ab920be6a65bfabdcab 100644 --- a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h +++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h @@ -23,6 +23,8 @@ #pragma once +#include "DiffIKProvider.h" + #include <VirtualRobot/Nodes/RobotNode.h> #include <VirtualRobot/RobotNodeSet.h> @@ -109,4 +111,19 @@ namespace armarx ///@brief Use this to check a trajectory of waypoints static Reachability CalculateReachability(const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters()); }; + + class SimpleDiffIKProvider : + public DiffIKProvider + { + public: + SimpleDiffIKProvider(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters()); + DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose); + DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues); + + private: + VirtualRobot::RobotNodeSetPtr rns; + VirtualRobot::RobotNodePtr tcp; + SimpleDiffIK::Parameters params; + }; } + diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp index e6987e894a504547847e1afd546dbb3af8daa254..557e31e5eb860f0e170a684aa6e7afa0b00487d4 100644 --- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp +++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp @@ -33,6 +33,7 @@ using namespace armarx; + CartesianNaturalPositionControllerProxy::CartesianNaturalPositionControllerProxy(const NaturalIK& ik, const NaturalIK::ArmJoints& arm, const RobotUnitInterfacePrx& robotUnit, const std::string& controllerName, NJointCartesianNaturalPositionControllerConfigPtr config) : _ik(ik), _robotUnit(robotUnit), _controllerName(controllerName), _config(config), _arm(arm) { @@ -87,7 +88,10 @@ void CartesianNaturalPositionControllerProxy::init() _controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl); _controllerCreated = true; } - _controller->activateController(); + if (_activateControllerOnInit) + { + _controller->activateController(); + } } bool CartesianNaturalPositionControllerProxy::setTarget(const Eigen::Matrix4f& tcpTarget, NaturalDiffIK::Mode setOri, std::optional<float> minElbowHeight) @@ -349,6 +353,11 @@ std::vector<float> CartesianNaturalPositionControllerProxy::ScaleVec(const std:: return result; } +void CartesianNaturalPositionControllerProxy::setActivateControllerOnInit(bool value) +{ + _activateControllerOnInit = value; +} + void CartesianNaturalPositionControllerProxy::setMaxVelocities(float referencePosVel) { VelocityBaseSettings& v = _velocityBaseSettings; diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h index eb75ba24394fb6deb1ae836b195e788d82957624..c2ccc06f5ce8b1a43106443ff3c169d0e12aa4e3 100644 --- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h +++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h @@ -114,7 +114,6 @@ namespace armarx VirtualRobot::RobotNodeSetPtr rns; }; - CartesianNaturalPositionControllerProxy(const NaturalIK& _ik, const NaturalIK::ArmJoints& arm, const RobotUnitInterfacePrx& _robotUnit, const std::string& _controllerName, NJointCartesianNaturalPositionControllerConfigPtr _config); static NJointCartesianNaturalPositionControllerConfigPtr MakeConfig(const std::string& rns, const std::string& elbowNode); @@ -175,6 +174,8 @@ namespace armarx DynamicKp getDynamicKp(); static std::vector<float> ScaleVec(const std::vector<float>& vec, float scale); + void setActivateControllerOnInit(bool value); + private: void updateDynamicKp(); void updateNullspaceTargets(); @@ -208,5 +209,6 @@ namespace armarx std::map<std::string, WaypointConfig> _defaultWaypointConfigs; bool _waypointChanged = false; FTSensorValue _ftOffset; + bool _activateControllerOnInit = false; }; } diff --git a/source/RobotAPI/libraries/natik/NaturalIK.cpp b/source/RobotAPI/libraries/natik/NaturalIK.cpp index 88c57b35768281fee04794b07cf789318203f140..f0ad1b7309e7f33c84764363ca5cd150a370b18b 100644 --- a/source/RobotAPI/libraries/natik/NaturalIK.cpp +++ b/source/RobotAPI/libraries/natik/NaturalIK.cpp @@ -213,3 +213,33 @@ void NaturalIK::setLowerArmLength(float value) { lowerArmLength = value; } + +NaturalIKProvider::NaturalIKProvider(const NaturalIK& natik, const NaturalIK::ArmJoints& arm, const NaturalDiffIK::Mode& setOri, const NaturalIK::Parameters& params) + : natik(natik), arm(arm), setOri(setOri), params(params) +{ +} + +DiffIKResult NaturalIKProvider::SolveAbsolute(const Eigen::Matrix4f& targetPose) +{ + params.diffIKparams.resetRnsValues = true; + NaturalDiffIK::Result result = natik.calculateIK(targetPose, arm, setOri, params); + DiffIKResult r; + r.jointValues = arm.rns->getJointValuesEigen(); + r.oriError = result.oriError; + r.posError = result.posError; + r.reachable = result.reached; + return r; +} + +DiffIKResult NaturalIKProvider::SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues) +{ + params.diffIKparams.resetRnsValues = false; + arm.rns->setJointValues(startJointValues); + NaturalDiffIK::Result result = natik.calculateIK(targetPose, arm, setOri, params); + DiffIKResult r; + r.jointValues = arm.rns->getJointValuesEigen(); + r.oriError = result.oriError; + r.posError = result.posError; + r.reachable = result.reached; + return r; +} diff --git a/source/RobotAPI/libraries/natik/NaturalIK.h b/source/RobotAPI/libraries/natik/NaturalIK.h index 6db3aa965d5860f4bb5bc1e2bca48901b08e8b04..d6315096a58084776797e6ce5551b65651ff4026 100644 --- a/source/RobotAPI/libraries/natik/NaturalIK.h +++ b/source/RobotAPI/libraries/natik/NaturalIK.h @@ -27,6 +27,7 @@ //#include <RobotAPI/libraries/core/SimpleDiffIK.h> #include <VirtualRobot/Nodes/RobotNode.h> +#include <RobotAPI/libraries/diffik/DiffIKProvider.h> #include <RobotAPI/libraries/diffik/NaturalDiffIK.h> #include <optional> @@ -134,4 +135,19 @@ namespace armarx float upperArmLength = SoechtingAngles::MMM_UPPER_ARM_LENGTH; float lowerArmLength = SoechtingAngles::MMM_LOWER_ARM_LENGTH; }; + + class NaturalIKProvider + : public DiffIKProvider + { + public: + NaturalIKProvider(const NaturalIK& natik, const NaturalIK::ArmJoints& arm, const NaturalDiffIK::Mode& setOri, const NaturalIK::Parameters& params = NaturalIK::Parameters()); + DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose); + DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues); + + private: + NaturalIK natik; + NaturalIK::ArmJoints arm; + NaturalDiffIK::Mode setOri; + NaturalIK::Parameters params; + }; }