outsource parsing model to another function

c++/CMakeLists.txt

@@ -14,9 +14,9 @@ find_package (broccoli 3.0.0 COMPONENTS eigen REQUIRED)
 find_package(RL COMPONENTS MDL REQUIRED)
 
 # dependencies for gpm function
-add_library(gpm SHARED src/gpm.cpp)
+add_library(my_funcs SHARED src/gpm.cpp src/loadKinematicModel.cpp)
 target_link_libraries (
-    gpm 
+    my_funcs 
     ${RL_LIBRARIES}
     eat::broccoli
     Eigen3::Eigen
@@ -27,12 +27,12 @@ add_subdirectory(deps/pybind11) # include pybind11 as a submodule
 pybind11_add_module(invKin src/py2gpmbind.cpp)
 target_link_libraries(
     invKin PUBLIC 
-    gpm
+    my_funcs
     ${RL_LIBRARIES}
     eat::broccoli
 )
 
-add_executable(myMain src/main.cpp src/gpm.cpp)
+add_executable(myMain src/main.cpp src/gpm.cpp src/loadKinematicModel.cpp)
 target_link_libraries(
     myMain
     eat::broccoli

c++/include/gpm.hpp

 #include <Eigen/Eigen>
+#include <rl/mdl/Kinematic.h>
 
 
 std::pair<Eigen::Matrix<double, 7, 1>, Eigen::Matrix<double, 6, 1>> gpm(Eigen::Matrix<double, 6, 1> &desPose, Eigen::Matrix<double, 6, 1> &desVelocity, 
-Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelocity, const bool left_arm);
\ No newline at end of file
+Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelocity, rl::mdl::Kinematic * kinematicPtr);

c++/include/loadKinematicModel.hpp

+#include <rl/mdl/Kinematic.h>
+
+rl::mdl::Kinematic * loadKinematicModel(std::string path);

c++/src/gpm.cpp

@@ -11,27 +11,12 @@
 
 
 std::pair<Eigen::Matrix<double, 7, 1>, Eigen::Matrix<double, 6, 1>> gpm(Eigen::Matrix<double, 6, 1> &desPose, Eigen::Matrix<double, 6, 1> &desVelocity, 
-Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelocity, const bool left_arm) {
+Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelocity, rl::mdl::Kinematic * kinematic) {
 
-	// FORWARD KINEMATICS
-	rl::mdl::UrdfFactory factory;
-	std::string path2urdf;
-
-	const clock_t t0 = clock();
 
+	// FORWARD KINEMATICS
 
-	if(left_arm){
-		path2urdf = "/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf";
-	} else{
-		path2urdf = "/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf";
-	}
-
-	std::shared_ptr<rl::mdl::Model> model(factory.create(path2urdf));
-
-	rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(model.get());
-
-	const clock_t t1 = clock();
-	std::cout << "time for parsing model: \t" << t1-t0 << std::endl;
+	//rl::mdl::Kinematic* kinematic = kinematicPtr;
 
 	// forward kinematics for the right arm
 	kinematic->setPosition(jointAngles);
@@ -47,7 +32,6 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
 	}
 
 	const clock_t t2 = clock();
-	std::cout << "time for copy jacobi: \t" << t2-t1 << std::endl;
 
 	// check if matrices are the same
 	//std::cout << "RLJacobian \n" << kinematic->getJacobian() << std::endl;

c++/src/loadKinematicModel.cpp

+#include <rl/mdl/Model.h>
+#include <rl/mdl/UrdfFactory.h>
+#include <rl/mdl/Kinematic.h>
+
+
+rl::mdl::Kinematic * loadKinematicModel(std::string path) {
+	rl::mdl::UrdfFactory factory;
+
+    // demo code but should be the same: https://github.com/roboticslibrary/rl/blob/master/demos/rlJacobianDemo/rlJacobianDemo.cpp
+	//std::shared_ptr<rl::mdl::Kinematic> kinematic;
+    //kinematic = std::dynamic_pointer_cast<rl::mdl::Kinematic>(factory.create(path));
+    
+    std::shared_ptr<rl::mdl::Model> model(factory.create(path));
+    
+
+	return dynamic_cast<rl::mdl::Kinematic*>(model.get());
+    //return kinematic;
+}

c++/src/main.cpp

 #include <iostream>
 #include "gpm.hpp"
+#include "loadKinematicModel.hpp"
 
 #include <Eigen/Eigen>
 #include <rl/math/Unit.h>
+#include <rl/mdl/Kinematic.h>
+#include <rl/mdl/Model.h>
+#include <rl/mdl/UrdfFactory.h>
 
 
 int main(int, char**) {
 
 	enum yumi_arm{YUMI_RIGHT, YUMI_LEFT};
 
+	//rl::mdl::Kinematic *kinematic_ptr = loadKinematicModel("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf");
+
+	rl::mdl::UrdfFactory factory;
+	std::shared_ptr<rl::mdl::Model> model(factory.create("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf"));
+	rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(model.get());
+
 	// Is Values
 	Eigen::Matrix<double, 6, 1> actualPosition;
 	Eigen::Matrix<double, 7, 1> jointAngles;
@@ -27,7 +37,7 @@ int main(int, char**) {
 	desVelocity << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
 
 	std::pair<Eigen::Matrix<double, 7, 1>, Eigen::Matrix<double, 6, 1>> result;
-	result = gpm(desPose, desVelocity, jointAngles, jointVelocity, YUMI_RIGHT);
+	result = gpm(desPose, desVelocity, jointAngles, jointVelocity, kinematic);
 	
 	std::cout << "desired joint values: \n" << result.first << std::endl;
 	std::cout << "current pose: \n" << result.second << std::endl;

c++/src/py2gpmbind.cpp

 #include <pybind11/pybind11.h>
 #include <pybind11/eigen.h>
 #include "gpm.hpp"
+#include "loadKinematicModel.hpp"
 
 namespace py = pybind11;
 
@@ -9,4 +10,8 @@ PYBIND11_MODULE(invKin, m) {
     m.def("gpm", &gpm, "A function to compute the invserse kinematics for a 7 DOF serial manipulator on vecocity level with comfort pose and manipulabilty gradient",
     py::arg("desired pose"),py::arg("desired Velocities"), py::arg("joint angles"), py::arg("joint velocities"), py::arg("left arm -> 1, right arm -> 0"), py::return_value_policy::copy);
 	
+    // load model and return ptr for future function calls
+    // https://pybind11.readthedocs.io/en/stable/advanced/functions.html#return-value-policies
+    m.def("loadKinematicModel", &loadKinematicModel, "Load model from URDF", py::arg("Path to URDF file"), py::return_value_policy::move);
+
 	}
\ No newline at end of file

python/abb_egm_pyclient/abb_egm_pyclient/feedforward_online/send2robot.py

@@ -6,6 +6,7 @@ import numpy as np
 from abb_egm_pyclient import EGMClient
 from data.get_data import get_desJoints_L, get_desJoints_R
 from libs.invKin import gpm
+from libs.invKin import loadKinematicModel
 from data.get_data import get_trajectory, transform2yumi_workspace, place_trajectory, Yumi, logic2abb
 
 '''
@@ -31,6 +32,9 @@ rate = 80
 egm_client_L = EGMClient(port=UDP_PORT_LEFT)
 egm_client_R = EGMClient(port=UDP_PORT_RIGHT)
 
+kinematic_model_ptr_L = loadKinematicModel("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf")
+kinematic_model_ptr_R = loadKinematicModel("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf")
+
 jointVelocities_L = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
 jointVelocities_R = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
 
@@ -75,8 +79,8 @@ for index, (pos1, vel1, pos2, vel2, phi, phi_dot) in enumerate(zip(p1, v1, p2, v
     ex_time = t3-t2
     print(f'computation time for assembling arrays: {ex_time}')
 
-    ik_jointAngles_L, ik_pose_L = gpm(desPose_L, desVelocities_L, jointAngles_L, jointVelocities_L, Yumi.LEFT.value)
-    ik_jointAngles_R, ik_pose_R = gpm(desPose_R, desVelocities_R, jointAngles_R, jointVelocities_R, Yumi.RIGHT.value)
+    ik_jointAngles_L, ik_pose_L = gpm(desPose_L, desVelocities_L, jointAngles_L, jointVelocities_L, kinematic_model_ptr_L)
+    ik_jointAngles_R, ik_pose_R = gpm(desPose_R, desVelocities_R, jointAngles_R, jointVelocities_R, kinematic_model_ptr_R)
 
     t4 = time.time()
     ex_time = t4-t3