Add FFI for JointPathEx

rtc/ImpedanceController/CMakeLists.txt

@@ -12,7 +12,10 @@ target_link_libraries(testImpedanceOutputGenerator ${libs})
 add_executable(testObjectTurnaroundDetector testObjectTurnaroundDetector.cpp ObjectTurnaroundDetector.h ../TorqueFilter/IIRFilter.cpp)
 target_link_libraries(testObjectTurnaroundDetector ${libs})
 
-set(target ImpedanceController ImpedanceControllerComp testImpedanceOutputGenerator testObjectTurnaroundDetector)
+add_library(JointPathExC SHARED JointPathExC.cpp JointPathEx.cpp)
+target_link_libraries(JointPathExC ${libs})
+
+set(target ImpedanceController ImpedanceControllerComp testImpedanceOutputGenerator testObjectTurnaroundDetector JointPathExC)
 
 add_test(testImpedanceOutputGeneratorTest0 testImpedanceOutputGenerator --test0 --use-gnuplot false)
 add_test(testImpedanceOutputGeneratorTest1 testImpedanceOutputGenerator --test1 --use-gnuplot false)

rtc/ImpedanceController/JointPathExC.cpp

@@ -0,0 +1,93 @@
+// -*- C++ -*-
+/*!
+ * @file JointPathExC.cpp
+ * @brief Standalone component
+ * @date $Date$
+ *
+ * $Id$
+ */
+
+#include <rtm/Manager.h>
+#include <rtm/CorbaNaming.h>
+#include <hrpModel/Body.h>
+#include <hrpModel/Link.h>
+#include <hrpModel/Sensor.h>
+#include <hrpModel/ModelLoaderUtil.h>
+#include "JointPathEx.h"
+
+
+extern "C" {
+    hrp::JointPathExPtr jpe;
+    hrp::BodyPtr m_robot(new hrp::Body());
+    static std::string print_prefix("[jpe]");
+
+    int initializeOpenHRPModel (char* _filename)
+    {
+        int rtmargc=0;
+        char** argv=NULL;
+        //std::vector<char *> rtmargv;
+        //rtmargv.push_back(argv[0]);
+        rtmargc++;
+
+        RTC::Manager* manager;
+        //manager = RTC::Manager::init(rtmargc, rtmargv.data());
+        manager = RTC::Manager::init(rtmargc, argv);
+
+        std::string nameServer = manager->getConfig()["corba.nameservers"];
+        int comPos = nameServer.find(",");
+        if (comPos < 0){
+            comPos = nameServer.length();
+        }
+        nameServer = nameServer.substr(0, comPos);
+        RTC::CorbaNaming naming(manager->getORB(), nameServer.c_str());
+        std::string filename(_filename);
+        if (!loadBodyFromModelLoader(m_robot, filename.c_str(),
+                                     CosNaming::NamingContext::_duplicate(naming.getRootContext()),
+                                     true)){
+            std::cerr << print_prefix << " Failed to load model[" << filename << "]" << std::endl;
+            return 1;
+        } else {
+            std::cerr << print_prefix << " Success to load model[" << filename << "]" << std::endl;
+        }
+        return 0;
+    };
+
+    int initializeJointPathExInstance (char* root_link_name, char* target_link_name)
+    {
+        jpe = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(root_link_name), m_robot->link(target_link_name), 0.002, false, std::string("test")));
+        if ( !jpe ) {
+            std::cerr << print_prefix << " Fail to joint path from " << root_link_name << " to " << target_link_name << std::endl;
+        } else {
+            std::cerr << print_prefix << " Success to joint path from " << root_link_name << " to " << target_link_name << " (dof = " << jpe->numJoints() << std::endl;
+        }
+        return 0;
+    }
+
+    int _setJointAngles (double* ja)
+    {
+        for ( int i = 0; i < m_robot->numJoints(); i++ ) {
+            m_robot->joint(i)->q = ja[i];
+        }
+        m_robot->calcForwardKinematics();
+        return 0;
+    }
+
+    int _getJointAngles (double* ja)
+    {
+        for ( int i = 0; i < m_robot->numJoints(); i++ ) {
+            ja[i] = m_robot->joint(i)->q;
+        }
+    }
+
+    int _calcInverseKinematics2Loop (double* _vel_p, double* _vel_r)
+    {
+        hrp::dvector qrefv = hrp::dvector::Zero(jpe->numJoints());
+        hrp::Vector3 vel_p, vel_r;
+        for (size_t i = 0; i < 3; i++) {
+            vel_p[i] = _vel_p[i];
+            vel_r[i] = _vel_r[i];
+        }
+        jpe->calcInverseKinematics2Loop(vel_p, vel_r, 1.0, 0.002, 0.0, &qrefv);
+        return 0;
+    }
+};

sample/euslisp/eus-joint-path-ex.l

@@ -0,0 +1,51 @@
+(defvar *libjointpathexc* (load-foreign (format nil "~A/../../lib/libJointPathExC.so" (ros::resolve-ros-path "package://hrpsys"))))
+
+(defforeign _initializeOpenHRPModel
+  *libjointpathexc*
+  "initializeOpenHRPModel"
+  (:string) ;; _filename
+  :integer)
+
+(defforeign _initializeJointPathExInstance
+  *libjointpathexc*
+  "initializeJointPathExInstance"
+  (:string  ;; root link
+   :string) ;; target link
+  :integer)
+
+(defforeign _setJointAngles
+  *libjointpathexc*
+  "_setJointAngles"
+  (:string) ;; ja
+  :integer)
+
+(defforeign _getJointAngles
+  *libjointpathexc*
+  "_getJointAngles"
+  (:string) ;; ja
+  :integer)
+
+(defforeign _calcInverseKinematics2Loop
+  *libjointpathexc*
+  "_calcInverseKinematics2Loop"
+  (:string :string) ;; vel_p, vel_r
+  :integer)
+
+#|
+(defun test-joint-path-ex ()
+  (_initializeOpenHRPModel (format nil "~A/../OpenHRP-3.1/sample/model/sample1.wrl" (ros::resolve-ros-path "package://openhrp3")))
+  (_initializeJointPathExInstance "WAIST" "LLEG_ANKLE_R")
+  (load "package://hrpsys_ros_bridge/models/samplerobot.l")
+  (objects (list (setq *robot* (samplerobot))))
+  (_setJointAngles (map float-vector #'deg2rad (send *robot* :angle-vector)))
+  (setq av (instantiate float-vector (length (send *robot* :angle-vector))))
+  (_getJointAngles av)
+  (send *robot* :angle-vector (map float-vector #'rad2deg av))
+  (do-until-key
+   (_calcInverseKinematics2Loop #f(0 0 0.001) #F(0 0 0))
+   (_getJointAngles av)
+   (send *robot* :angle-vector (map float-vector #'rad2deg av))
+   (send *irtviewer* :draw-objects))
+  )
+#|
+