diff --git a/c++/gpm.cpp b/c++/gpm.cpp
index 21313a26526d2f33ccc0eb6a39fab9c074eface4..e3705e46a964b1b774ed4af44e372c3c91b1a82d 100644
--- a/c++/gpm.cpp
+++ b/c++/gpm.cpp
@@ -113,6 +113,8 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
 	effectiveTaskSpaceInput.head(3) = gainDriftCompensation/dt * (desiredTranslation - currentTranslation)
 										+ desVelocity.head(3);
 	effectiveTaskSpaceInput.tail(3) = gainDriftCompensation/dt * errorRotationInWorldFrame + desVelocity.tail(3);
+	std::cout << "effectiveTaskSpaceInput: " << effectiveTaskSpaceInput << std::endl;
+
 
 	// COMPUTE GRADIENTS
 	// define min and max values for the joints of Yumi
@@ -172,12 +174,13 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
 	Eigen::Matrix<double, 7, 7>  m_inverseWeighing = Eigen::Matrix<double, 7, 7> ::Identity();
 	double m_activationFactorTaskSpace = 1.0;
 	Eigen::Matrix<double, 7, 1> nullSpaceVelocity = -m_inverseWeighing * nullSpaceGradient;
-	broccoli::core::math::solvePseudoInverseEquation(J, m_inverseWeighing, effectiveTaskSpaceInput, nullSpaceVelocity, m_activationFactorTaskSpace);
+	Eigen::Matrix<double, 7, 1> jointVelocities;
+	jointVelocities = broccoli::core::math::solvePseudoInverseEquation(J, m_inverseWeighing, effectiveTaskSpaceInput, nullSpaceVelocity, m_activationFactorTaskSpace);
 	
+	std::cout << "resulting jointVelocities: \n" << jointVelocities << std::endl;
 	// perform integration over one timestep to obtain positions that can be send to robot
-	jointAnglesDelta << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
-	// integrate value over one time step
-	jointAnglesDelta *= dt;
+	jointAnglesDelta << jointVelocities * dt;
+
 
 	//std::cout << "current qs in DEG \n" << jointAngles* rl::math::RAD2DEG << std::endl;
 	std::cout << "delta qs in DEG \n" << jointAnglesDelta * rl::math::RAD2DEG << std::endl;
diff --git a/python/taskspace_placement/computeJoints_left.py b/python/taskspace_placement/computeJoints_left.py
index 10b74325e712e1307218ab54ec3b91bfb896f7de..7b1e4f403e282ea1d7eaab1d8d0c322c9425a01e 100644
--- a/python/taskspace_placement/computeJoints_left.py
+++ b/python/taskspace_placement/computeJoints_left.py
@@ -64,8 +64,8 @@ computedPose = np.zeros((len(p1[:,0]),6))
 error = np.zeros((len(p1[:,0]),6))
 
 for index, (pos, vel, phi, phi_dot) in enumerate(zip(p1, v1, phi_total, dphi)): # loop through all the desired position of left arm
-    desPose = np.concatenate((pos, phi_const), axis=0) # note that phi_const is used -> same orientation throughout the movement
-    desVelocities = np.concatenate((vel, dphi_const), axis=0) # same here
+    desPose = np.concatenate((pos, phi), axis=0) # note that phi_const is used -> same orientation throughout the movement
+    desVelocities = np.concatenate((vel, phi_dot), axis=0) # same here
     # call the c++ egm function, return joint values and resulting pose
     result = invKin.gpm(desPose, desVelocities, jointAngles, jointVelocities, 1)
     desJointAngles[index,:] = result[0] # computed joint values from IK
@@ -113,6 +113,14 @@ plt.legend()
 plt.title('view on the x-z plane from the right arm side of yumi')
 plt.show()
 
+fig = plt.figure()
+plt.plot(computedPose[:,3], label='rx')
+plt.plot(computedPose[:,4], label='ry')
+plt.plot(computedPose[:,5], label='rz')
+plt.legend()
+plt.title('euler angles over trajectories')
+plt.show()
+
 np.save('desJointAngles_left', desJointAngles)