diff --git a/c++/include/gpm.hpp b/c++/include/gpm.hpp
index ca063b958908ed495248287a64d2c18d8b39fd36..4d367ee8fe7bd37f4b59e8723fc6fa5305cabd32 100644
--- a/c++/include/gpm.hpp
+++ b/c++/include/gpm.hpp
@@ -3,4 +3,4 @@
 
 
 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, void * kinematic_ptr);
+Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelocity, std::shared_ptr<rl::mdl::Model> model);
diff --git a/c++/include/loadKinematicModel.hpp b/c++/include/loadKinematicModel.hpp
index ca6d3ad89a8065de7bca25333817ecceeea3fc3e..b6be6d077a34fe2110592ffb0dfc54b6c7bcbf6e 100644
--- a/c++/include/loadKinematicModel.hpp
+++ b/c++/include/loadKinematicModel.hpp
@@ -4,10 +4,9 @@ class Arm {
     public:
     Arm(std::string path);
 
-    void* get_pointer2arm();
+    std::shared_ptr<rl::mdl::Model> get_pointer2arm();
 
     private:
-    rl::mdl::Kinematic* kin_pointer;
-    rl::mdl::Kinematic kin_model;
-    void* void_pointer;
+    //rl::mdl::Kinematic kin_model;
+    std::shared_ptr<rl::mdl::Model> model;
  };
diff --git a/c++/src/gpm.cpp b/c++/src/gpm.cpp
index 69674105660c001cee58daeb513f9ff203c8ad84..4aac2751893a816ed9a4080254fb81f40a179063 100644
--- a/c++/src/gpm.cpp
+++ b/c++/src/gpm.cpp
@@ -11,12 +11,11 @@
 
 
 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, void * kinematic_ptr) {
+Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelocity, std::shared_ptr<rl::mdl::Model> model) {
 
+	rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(model.get());
 
 	// FORWARD KINEMATICS
-	rl::mdl::Kinematic* kinematic = (rl::mdl::Kinematic*) kinematic_ptr;
-	std::cout << "adress of pointer gpm " << kinematic_ptr << std::endl;
 	
 
 	//std::cout << "pointer in gpm" << kinematic_ptr << std::endl;
diff --git a/c++/src/loadKinematicModel.cpp b/c++/src/loadKinematicModel.cpp
index 553ad68b91d19ff4fc49d0c527ebe6108fd84b78..856f00af1bae7364f73f7ce67541035008e3e00e 100644
--- a/c++/src/loadKinematicModel.cpp
+++ b/c++/src/loadKinematicModel.cpp
@@ -7,15 +7,15 @@
 
 Arm::Arm(std::string path){
     rl::mdl::UrdfFactory factory;
-    std::shared_ptr<rl::mdl::Model> model(factory.create(path));
-    kin_pointer = dynamic_cast<rl::mdl::Kinematic*>(model.get());
-    kin_model = *(kin_pointer);
-    rl::mdl::Kinematic* tmp_pointer = &kin_model;
-    void_pointer = (void*) tmp_pointer;
-    std::cout << "adress of pointer load " << void_pointer << std::endl;
+    std::shared_ptr<rl::mdl::Model> model_tmp(factory.create(path));
+    model = model_tmp;
+    
+    
+    //std::cout << "adress of pointer load " << void_pointer << std::endl;
     }
 
-void* Arm::get_pointer2arm() {return void_pointer;}
+std::shared_ptr<rl::mdl::Model> Arm::get_pointer2arm() {return model;}
+
 
 /* class Arm {
     public:
diff --git a/c++/src/main.cpp b/c++/src/main.cpp
index b6a42286f31da16631b44a98b78aa1e23cb02bb2..43be50a532d9bcfabc7ffff62de23767ff0f2f3c 100644
--- a/c++/src/main.cpp
+++ b/c++/src/main.cpp
@@ -24,7 +24,7 @@ int main(int, char**) {
 	rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(model.get());
 
 	Arm right_arm("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf");
-	void* test_pointer = right_arm.get_pointer2arm();
+	std::shared_ptr<rl::mdl::Model> abc = right_arm.get_pointer2arm();
 
 	// Is Values
 	Eigen::Matrix<double, 6, 1> actualPosition;
@@ -44,7 +44,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, test_pointer);
+	result = gpm(desPose, desVelocity, jointAngles, jointVelocity, abc);
 	
 	std::cout << "desired joint values: \n" << result.first << std::endl;
 	std::cout << "current pose: \n" << result.second << std::endl;