Added PlanarLinkage task to outomatic discover planar linkages. Added…

… working example

CMakeLists.txt

@@ -44,7 +44,8 @@ include_directories(include
     ${catkin_INCLUDE_DIRS})
 
 add_library(ClosedSoT SHARED
-    src/tasks/velocity/ClosedChain.cpp)
+    src/tasks/velocity/ClosedChain.cpp
+    src/tasks/LinkageFactory.cpp)
 
 target_link_libraries(ClosedSoT
     ${catkin_LIBRARIES} ${OpenSoT_LIBRARIES} ${VISP_LIBRARIES})
@@ -56,7 +57,8 @@ install(TARGETS ClosedSoT
         )
 
 add_library(CartesioClosedSoT SHARED
-                cartesio/tasks/velocity/ClosedChain.cpp)
+                cartesio/tasks/velocity/ClosedChain.cpp
+                cartesio/tasks/velocity/PlanarLinkage.cpp)
 
 target_link_libraries(CartesioClosedSoT ClosedSoT ${cartesian_interface_LIBRARIES} ${catkin_LIBRARIES})
 

cartesio/tasks/velocity/PlanarLinkage.cpp

@@ -0,0 +1,63 @@
+#include "PlanarLinkage.h"
+
+using namespace XBot::Cartesian::velocity;
+
+std::string task_name(YAML::Node node)
+{
+    auto distal_link = node["distal_link"].as<std::string>();
+    auto base_link = node["base_link"].as<std::string>();
+    return "planar_linkage_"+distal_link+base_link;
+}
+
+PlanarLinkageImpl::PlanarLinkageImpl(YAML::Node node, Context::ConstPtr context):
+    TaskDescriptionImpl (node, context, task_name(node), 2)
+{
+    _base_link = node["base_link"].as<std::string>();
+    _distal_link = node["distal_link"].as<std::string>();
+}
+
+const std::string& PlanarLinkageImpl::getBaseLink() const
+{
+    return _base_link;
+}
+
+const std::string& PlanarLinkageImpl::getDistalLink() const
+{
+    return _distal_link;
+}
+
+
+ClosedSotPlanarLinkageAdapter::ClosedSotPlanarLinkageAdapter(TaskDescription::Ptr ci_task, Context::ConstPtr context):
+    OpenSotTaskAdapter(ci_task, context)
+{
+    _ci_pl = std::dynamic_pointer_cast<PlanarLinkageTask>(ci_task);
+    if(!_ci_pl) throw std::runtime_error("Provided task description "
+                                            "does not have expected type 'PlanarLinkage'");
+    _linkage_factory = std::make_shared<ClosedSoT::tasks::LinkageFactory>();
+}
+
+TaskPtr ClosedSotPlanarLinkageAdapter::constructTask()
+{
+    Eigen::VectorXd q;
+    _model->getJointPosition(q);
+
+    _closedsot_cc = std::dynamic_pointer_cast<ClosedSoT::tasks::velocity::ClosedChain>(_linkage_factory->createPlanarLinkageTask(const_cast<ModelInterface&>(*_model),
+                                                                                                                                 _ci_pl->getDistalLink(), _ci_pl->getBaseLink()));
+    _closedsot_cc->cartesian_task->setLambda(_ci_pl->getLambda());
+
+    return _closedsot_cc;
+}
+
+void ClosedSotPlanarLinkageAdapter::processSolution(const Eigen::VectorXd& solution)
+{
+    OpenSotTaskAdapter::processSolution(solution);
+}
+
+void ClosedSotPlanarLinkageAdapter::update(double time, double period)
+{
+    _closedsot_cc->cartesian_task->setLambda(_ci_pl->getLambda());
+}
+
+CARTESIO_REGISTER_TASK_PLUGIN(PlanarLinkageImpl, PlanarLinkage)
+CARTESIO_REGISTER_OPENSOT_TASK_PLUGIN(ClosedSotPlanarLinkageAdapter, PlanarLinkage)
+

cartesio/tasks/velocity/PlanarLinkage.h

@@ -0,0 +1,70 @@
+#ifndef _CARTESIO_PLANARLINKAGE_H_
+#define _CARTESIO_PLANARLINKAGE_H_
+
+#include <cartesian_interface/sdk/problem/Interaction.h>
+#include <cartesian_interface/sdk/ros/server_api/TaskRos.h>
+#include <cartesian_interface/sdk/ros/client_api/TaskRos.h>
+
+#include <cartesian_interface/sdk/opensot/OpenSotTask.h>
+
+#include <ClosedSoT/tasks/LinkageFactory.h>
+
+using CSoT = ClosedSoT::tasks::velocity::ClosedChain;
+
+namespace XBot {
+    namespace Cartesian {
+        namespace velocity {
+
+        class PlanarLinkageTask: public virtual TaskDescription
+        {
+        public:
+            CARTESIO_DECLARE_SMART_PTR(PlanarLinkageTask)
+            virtual const std::string& getBaseLink() const = 0;
+            virtual const std::string& getDistalLink() const = 0;
+        };
+
+        class PlanarLinkageImpl: public virtual PlanarLinkageTask,
+                                  public TaskDescriptionImpl
+        {
+            public:
+                CARTESIO_DECLARE_SMART_PTR(PlanarLinkageImpl)
+
+                const std::string& getBaseLink() const override;
+                const std::string& getDistalLink() const override;
+
+                PlanarLinkageImpl(YAML::Node node, Context::ConstPtr context);
+        private:
+                std::string _base_link, _distal_link;
+        };
+
+        class ClosedSotPlanarLinkageAdapter : public OpenSotTaskAdapter
+        {
+
+        public:
+
+            ClosedSotPlanarLinkageAdapter(TaskDescription::Ptr ci_task, Context::ConstPtr context);
+
+            virtual TaskPtr constructTask() override;
+
+            virtual void update(double time, double period) override;
+
+            virtual void processSolution(const Eigen::VectorXd& solution) override;
+
+            virtual ~ClosedSotPlanarLinkageAdapter() override = default;
+
+        protected:
+
+            CSoT::Ptr _closedsot_cc;
+
+        private:
+            ClosedSoT::tasks::LinkageFactory::Ptr _linkage_factory;
+            PlanarLinkageTask::Ptr _ci_pl;
+
+        };
+
+        }
+    }
+}
+
+
+#endif

examples/launch/cartesio.launch

@@ -0,0 +1,47 @@
+<launch>
+    <!-- LOAD URDF AND SRDF IN PARAM SERVER -->
+    <param name="robot_description" command=" $(find xacro)/xacro --inorder '$(find closed_sot)/examples/urdf/four_bar_linkage.urdf.xacro'"/>
+    <param name="robot_description_semantic" command=" $(find xacro)/xacro --inorder '$(find closed_sot)/examples/srdf/four_bar_linkage.srdf.xacro'"/>
+
+    <!-- LOAD STACK IN PARAM SERVER -->
+    <param name="cartesian/problem_description" textfile="$(find closed_sot)/examples/stack/four_bar_linkage.stack"/>
+
+    <!-- RUN CARTESI/O -->
+    <arg name="solver" default="OpenSot"/>
+    <arg name="prefix" default=""/>
+    <arg name="use_xbot_config" default="false"/>
+    <arg name="verbosity" default="2"/>
+    <arg name="rate" default="500.0"/>
+    <arg name="tf_prefix" default="ci"/>
+    <arg name="markers" default="true"/>
+    <arg name="namespace" default=""/> <!-- dummy argument avoids pass_all_args error in parent launch file -->
+    <arg name="robot" default=""/>
+    <arg name="is_model_floating_base" default="false"/>
+
+
+    <node pkg="cartesian_interface" type="ros_server_node" 
+                                    name="ros_server_node" 
+                                    required="true" 
+                                    output="screen" 
+                                    launch-prefix="$(arg prefix)">
+
+        <param name="is_model_floating_base" value="$(arg is_model_floating_base)"/>
+        <param name="model_type" value="RBDL"/>
+        <param name="solver" value="$(arg solver)"/>
+        <param name="use_xbot_config" value="$(arg use_xbot_config)"/>
+        <param name="verbosity" value="$(arg verbosity)"/>
+        <param name="rate" value="$(arg rate)"/>
+        <param name="tf_prefix" value="$(arg tf_prefix)"/>
+
+    </node>
+
+    <!-- node pkg="tf" type="static_transform_publisher" name="world_static_transform" args="0 0 0 0 0 0 ci/world odometry/world 100" /-->
+
+    <!-- RUN POSTURAL GUI -->
+    <node pkg="cartesian_interface" type="postural_gui" name="postural_gui"/>
+
+    <!-- RVIZ -->
+    <node type="rviz" name="rviz" pkg="rviz" args="-d $(find kangaroo_full_model)/rviz/cartesio.rviz"/>
+
+
+</launch>

examples/srdf/four_bar_linkage.srdf.xacro

@@ -0,0 +1,23 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="four_bar_linkage">
+
+<xacro:include filename="$(find closed_sot)/utils/srdf/utils.srdf.xacro" />
+
+<group name="base">
+	<link name="base_link"/>
+</group>
+
+<xacro:planar_linkage base_frame="base_link" open_frame_a="frame0" open_frame_b="frame1"/>
+
+<group name="chains">
+        <group name="base_link_to_frame0"/>
+        <group name="base_link_to_frame1"/>
+</group>
+
+<group_state name="home" group="chains">
+    <joint name="Joint1" value="0.0"/>
+    <joint name="Joint2" value="0.0"/>
+    <joint name="Joint3" value="0.0"/>
+</group_state>
+
+</robot>

examples/stack/four_bar_linkage.stack

@@ -0,0 +1,25 @@
+solver_options:
+    regularization: 1e-3
+    back_end: "qpoases"
+
+stack:
+    - ["Postural"]
+
+constraints: ["JointLimits", "planar_linkage"]
+
+JointLimits:
+    type: "JointLimits"
+
+planar_linkage:
+    lib_name: libCartesioClosedSoT.so
+    type: "PlanarLinkage"
+    base_link: "frame0"
+    distal_link: "frame1"
+    lambda: 0.1
+
+Postural:
+    type: "Postural"
+    lambda: 0.1
+    disabled_joints:
+        - Joint2
+        - Joint3

examples/urdf/four_bar_linkage.urdf.xacro

@@ -7,7 +7,7 @@
 <xacro:property name="m" value="1." />
 <xacro:property name="PI" value="3.1415" />
 
-<link name="link0"/>
+<link name="world"/>
 
 <link name="base_link">
 	<inertial>
@@ -96,8 +96,8 @@
 <link name="frame0"/>
 <link name="frame1"/>
 
-<joint name="Joint0" type="fixed">
-	<parent link="link0"/>
+<joint name="base_joint" type="fixed">
+	<parent link="world"/>
 	<child link="base_link"/>
 	<origin rpy="0 0 0" xyz="0 0 0"/>
 </joint>

include/ClosedSoT/tasks/LinkageFactory.h

@@ -0,0 +1,27 @@
+#ifndef _CLOSEDSOT_LINKAGEFACTORY_H_
+#define _CLOSEDSOT_LINKAGEFACTORY_H_
+
+#include <ClosedSoT/tasks/velocity/ClosedChain.h>
+
+namespace ClosedSoT{
+namespace tasks {
+
+    class LinkageFactory
+    {
+    public:
+        typedef std::shared_ptr<LinkageFactory> Ptr;
+
+        LinkageFactory();
+
+        virtual ~LinkageFactory(){}
+
+        OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>::TaskPtr createPlanarLinkageTask(XBot::ModelInterface &robot,
+                                                                                         const std::string& distal_link,
+                                                                                         const std::string& base_link);
+
+    };
+
+}
+}
+
+#endif

src/tasks/LinkageFactory.cpp

@@ -0,0 +1,32 @@
+#include <ClosedSoT/tasks/LinkageFactory.h>
+
+using namespace ClosedSoT::tasks;
+
+LinkageFactory::LinkageFactory()
+{
+
+}
+
+OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>::TaskPtr LinkageFactory::createPlanarLinkageTask(XBot::ModelInterface &robot,
+                                                               const std::string& distal_link,
+                                                               const std::string& base_link)
+{
+    Eigen::MatrixXd J(6, robot.getJointNum());
+    J.setZero();
+    robot.getRelativeJacobian(distal_link, base_link, J);
+
+    std::list<unsigned int> constrained_dofs;
+    //for now we consider only position
+    for(unsigned int i = 0; i < 3; ++i)
+    {
+        if(!J.row(i).isZero())
+            constrained_dofs.push_back(i);
+    }
+
+    if(constrained_dofs.size() == 3)
+        throw std::runtime_error("Overconstrained! Can not create a Planar linkage!");
+
+    Eigen::VectorXd q(robot.getJointNum());
+    q.setZero();
+    return std::make_shared<velocity::ClosedChain>(q, robot, distal_link, base_link, constrained_dofs);
+}

utils/srdf/utils.srdf.xacro

@@ -0,0 +1,26 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+<!-- 
+	planar_linkage: creates two chain groups named "${base_frame}_to_{open_frame_a}" and "${base_frame}_to_{open_frame_b}" 
+		params: 
+			base_frame: common frame of the two open chain in the planar linkage
+			open_frame_a: final frame of first open chain
+			open_frame_b: final frame of second open chain
+-->
+<xacro:macro name="planar_linkage" params="base_frame open_frame_a open_frame_b">
+
+<group name="${base_frame}_to_${open_frame_a}">
+	<chain base_link="${base_frame}" tip_link="${open_frame_a}"/>
+</group>
+
+<group name="${base_frame}_to_${open_frame_b}">
+	<chain base_link="${base_frame}" tip_link="${open_frame_b}"/>
+</group>
+
+</xacro:macro>
+
+</robot>
+
+
+