diff --git a/c++/gpm.cpp b/c++/gpm.cpp
index 2f46d0c555e98a5c6101ddf1439c8bde86c74eba..76dfbac97bdcb68c1d4e2f556151f0b6ad458b30 100644
--- a/c++/gpm.cpp
+++ b/c++/gpm.cpp
@@ -23,7 +23,7 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
// instantiate vars vor Automatic Supervisory Control (ASC)
Eigen::Matrix<double, 7, 1> nullSpaceGradient = Eigen::Matrix<double, 7, 1>::Zero();
Eigen::Matrix<double, 7, 1> manipGradient = Eigen::Matrix<double, 7, 1>::Zero();
- Eigen::Matrix<double, 6, 1> actualPosition;
+ Eigen::Matrix<double, 6, 1> actualPose;
Eigen::Matrix<double, 6, 1> dPosition;
Eigen::Matrix<double, 6, 1> resPose;
const double dt = 0.0125; // refers to 80 Hz
@@ -85,12 +85,14 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
// INVERSE KINEMATICS
// obtained from forward kinematics, later the current configuration read from egm interface
- actualPosition << position.transpose()(0), position.transpose()(1), position.transpose()(2), orientation.transpose()(0), orientation.transpose()(1), orientation.transpose()(2);
+
+ //actualPose << position.transpose()(0), position.transpose()(1), position.transpose()(2), orientation.transpose()(0), orientation.transpose()(1), orientation.transpose()(2);
+ actualPose << position.transpose()(0), position.transpose()(1), position.transpose()(2), 106.0*rl::math::DEG2RAD, 90.0*rl::math::DEG2RAD, 106.0*rl::math::DEG2RAD;
// add 1 mm to the position part
dPosition << 0.0, 0.01, 0.0, 0.0, 0.0, 0.0;
- //desPose = actualPosition + dPosition;
+ //desPose = actualPose + dPosition;
//std::cout << "desPose \n" << desPose << std::endl;
- // apply vel that results from actualPosition and desPose
+ // apply vel that results from actualPose and desPose
//desVelocity << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
// TODO: this needs to be changed soon!
//desVelocity << dPosition * 1/dt;
@@ -143,12 +145,12 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
//ik.setWeighingMatrix(weightingFactors); // by default the identity matrix
//ik.setTaskSpaceConstraintFactor(activationFactor); // is set to 1 by default
// set feedback gain for effective desired velocity
- ik.setDriftCompensationGain(0.005); // set to 1 by default, 0.5 is still too high, 0.01 works
+ ik.setDriftCompensationGain(0.5); // set to 1 by default, 0.5 is still too high, 0.01 works
// set the target position and velocity
ik.setTarget(desPose, desVelocity); // needs to be specified in order to give reasonable results for ik.outoutVelocity
// compute the desired velocities in taskspace
- ik.process(J, actualPosition, nullSpaceGradient, dt);
+ ik.process(J, actualPose, nullSpaceGradient, dt);
// obtain the computed velocity in task space
//std::cout << "output ik \n" << ik.outputVelocity() << std::endl;
diff --git a/python/taskspace_placement/computeJoints_left.py b/python/taskspace_placement/computeJoints_left.py
index ec55c033586cb85269b870efb3eeb13771f1395b..8545332ba1452f1bdd29cec8f5d0a448d07a4d5c 100644
--- a/python/taskspace_placement/computeJoints_left.py
+++ b/python/taskspace_placement/computeJoints_left.py
@@ -41,29 +41,25 @@ for i in range(len(p1[:,0])):
# START CONFIGURATION FOR THE LEFT ARM
# set the joint angles that map to the desired start position - read from RobotStudio
-jointAngles = np.array([90.48, 17.87, -25.09, 48.0, -137.0, 122.0, -74.21]) * np.pi/180.0 #show good manipulability index in RS
#jointAngles = np.array([90.48, 17.87, -25.09, 48.0, -137.0, 122.0, -74.21]) * np.pi/180.0 #show good manipulability index in RS
-#jointAngles = np.array([90.48, 17.87, -25.09, 48.0, -137.0, 122.0, 15.79]) * np.pi/180.0
+#jointAngles = np.array([90.48, 17.87, -25.09, 48.0, -137.0, 122.0, -74.21]) * np.pi/180.0 #show good manipulability index in RS
+jointAngles = np.array([90.48, 17.87, -25.09, 48.0, -137.0, 122.0, 15.79]) * np.pi/180.0
# initial jointVelocites are zero
jointVelocities = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
dt = 0.0125
-phi_const = np.array([90.0, 180.0, 90.0]) * np.pi/180.0 # values that FK computed, keep angle the same for now, just care about positions
-#phi_const = np.array([106.0, 90.0, 106.0]) * np.pi/180.0
+#phi_const = np.array([90.0, 180.0, 90.0]) * np.pi/180.0 # values that FK computed, keep angle the same for now, just care about positions
+phi_const = np.array([106.0, 90.0, 106.0]) * np.pi/180.0
dphi_const = np.array([0.0, 0.0, 0.0]) * np.pi/180.0
-# the current position should match px[0,:] and the desired next position is px[1,:]
-desPose = np.concatenate((p1[1,:], phi_const), axis=0)
-desVelocities = np.concatenate((v1[1,:], dphi_const), axis=0)
-#desVelocities = np.concatenate((np.array([0, 0, 0]), dphi_const), axis=0)
-phi_base = np.zeros((len(p1[:,0]),3)) + (np.array([90.0, 180.0, 90.0])* np.pi/180.0)
+phi_base = np.zeros((len(p1[:,0]),3)) + phi_const
phi_total = phi_base + phi_delta
desJointAngles = np.zeros((len(p1[:,0]),7))
computedPose = np.zeros((len(p1[:,0]),6))
error = np.zeros((len(p1[:,0]),6))
-for index, (pos, vel) in enumerate(zip(p1, v1)): # loop through all the desired position of left arm
+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)
desVelocities = np.concatenate((vel, dphi_const), axis=0)
# call the c++ egm function, return joint values and resulting pose
@@ -76,6 +72,7 @@ for index, (pos, vel) in enumerate(zip(p1, v1)): # loop through all the desired
print('IK resulting pose', result[1])
print('\n error', desPose - result[1])
error[index, :] = desPose - result[1]
+ #error[index, 3:6] = np.array([0, 0, 0])
jointAngles = result[0]
# see development of joint values