diff --git a/c++/gpm.cpp b/c++/gpm.cpp
index 76dfbac97bdcb68c1d4e2f556151f0b6ad458b30..86dbed0900149c29f022e786dbefbcc00b0e5f7f 100644
--- a/c++/gpm.cpp
+++ b/c++/gpm.cpp
@@ -12,6 +12,7 @@
#include <rl/mdl/UrdfFactory.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 int arm = 0) {
/*
@@ -27,6 +28,7 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
Eigen::Matrix<double, 6, 1> dPosition;
Eigen::Matrix<double, 6, 1> resPose;
const double dt = 0.0125; // refers to 80 Hz
+ double rx, ry, rz;
// create ASC object and execute its functions for inverse kinematics
broccoli::control::AutomaticSupervisoryControl<6,7> ik;
@@ -71,9 +73,9 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
}
// check if matrices are the same
- std::cout << "RLJacobian \n" << kinematic->getJacobian() << std::endl;
- std::cout << "myJacobian \n" << J << std::endl;
- std::cout << "Manipulability meassure \n" << kinematic->calculateManipulabilityMeasure() << std::endl;
+ //std::cout << "RLJacobian \n" << kinematic->getJacobian() << std::endl;
+ //std::cout << "myJacobian \n" << J << std::endl;
+ //std::cout << "Manipulability meassure \n" << kinematic->calculateManipulabilityMeasure() << std::endl;
// extract orientation and position for the right arm
rl::math::Transform t = kinematic->getOperationalPosition(0);
@@ -82,22 +84,57 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
std::cout << "Joint configuration in degrees: " << jointAngles.transpose() * rl::math::RAD2DEG << std::endl;
std::cout << "FK end-effector position: [m] " << position.transpose() << " orientation [deg] " << orientation.transpose() * rl::math::RAD2DEG << std::endl;
+ std::cout << "CHECK ORIENTATION BEFORE " << orientation.x() * rl::math::RAD2DEG << " " << orientation.y() * rl::math::RAD2DEG << " " << orientation.z() * rl::math::RAD2DEG << std::endl;
+
+ std::cout << " desPose1 " << desPose[0,0] << " desPose2 " << desPose[0,1] << " desPose3 " << desPose[0,2]
+ << " desPose4 " << desPose[0,3] << " desPose5 " << desPose[0,4] << " desPose6 " << desPose[0,5] << std::endl;
+
+ // According to RL angles are in range [0:pi]x[-pi:pi]x[-pi:pi]
+ // jumping angles are an issue as the resulting computed error in ik.process() is not the real error
+ // therefore modify the angles such that the resulting error between desPose and actualPose corresponds to the real error
+ rx = orientation.x();
+ ry = orientation.y();
+ rz = orientation.z();
+
+ double bound_low = -M_PI * 17.0/18.0; // equals -170 deg in rad
+ double bound_low_x = M_PI * 1.0/18.0; // equals 10 deg in rad
+ double bound_high = M_PI * 17.0/18.0; // equals 170 deg in rad
+
+ if ((rz < bound_low) && (desPose[0, 5] > bound_high)){
+ rz = M_PI - (-M_PI - rz); // rz is negative and will be added to the positive range, ending at pi
+ } else if ((rz > bound_high) && (desPose[0, 5] < bound_low)){
+ rz = -M_PI - (M_PI - rz); // rz is positive and will be a the negative
+ }
+
+ if ((ry < bound_low) && (desPose[0, 4] > bound_high)){
+ ry = M_PI - (-M_PI - ry);
+ } else if ((ry > bound_high) && (desPose[0, 4] < bound_low)){
+ ry = -M_PI - (M_PI - ry);
+ }
+
+ if ((rx < bound_low_x) && (desPose[0, 3] > bound_high)){ // M_PI/4.0 is 45 deg
+ rx = M_PI + rx;
+ } else if ((rx > bound_high) && (desPose[0, 3] < bound_low_x)){
+ rx = - (M_PI - rx);
+ }
+
+ std::cout << "CHECK ORIENTATION AFTER " << rx * rl::math::RAD2DEG << " " << ry * rl::math::RAD2DEG << " " << rz * rl::math::RAD2DEG << std::endl;
// INVERSE KINEMATICS
// obtained from forward kinematics, later the current configuration read from egm interface
//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;
+ actualPose << 0.3, 0.2, 0.2, rx, ry, rz;
// add 1 mm to the position part
dPosition << 0.0, 0.01, 0.0, 0.0, 0.0, 0.0;
//desPose = actualPose + dPosition;
//std::cout << "desPose \n" << desPose << std::endl;
// apply vel that results from actualPose and desPose
- //desVelocity << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+ desVelocity << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
// TODO: this needs to be changed soon!
//desVelocity << dPosition * 1/dt;
- std::cout << "desPose \n" << desPose << std::endl;
- std::cout << "desVelocity \n" << desVelocity << std::endl;
+ //std::cout << "desPose \n" << desPose << std::endl;
+ //std::cout << "desVelocity \n" << desVelocity << std::endl;
// compute CPG gradient
cpg.useRangeBasedScaling(q_min, q_max);
@@ -139,7 +176,7 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
// add both gradients
nullSpaceGradient = 0*manipGradient + 0*cpgGradient;
- std::cout << "gradient \n" << nullSpaceGradient << std::endl;
+ //std::cout << "gradient \n" << nullSpaceGradient << std::endl;
// do the inverse kinematics
//ik.setWeighingMatrix(weightingFactors); // by default the identity matrix
@@ -158,13 +195,13 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
// perform integration over one timestep to obtain positions that can be send to robot
desq << ik.outputVelocity().value();
- std::cout << "output velocty in deg/s \n" << desq* rl::math::RAD2DEG << std::endl;
+ //std::cout << "output velocty in deg/s \n" << desq* rl::math::RAD2DEG << std::endl;
//std::cout << "test2 \n" << dq_r << std::endl;
desq *= dt;
- std::cout << "current qs in DEG \n" << jointAngles* rl::math::RAD2DEG << std::endl;
+ //std::cout << "current qs in DEG \n" << jointAngles* rl::math::RAD2DEG << std::endl;
std::cout << "delta qs in DEG \n" << desq * rl::math::RAD2DEG << std::endl;
- std::cout << "next qs in DEG \n" << (jointAngles+desq)* rl::math::RAD2DEG << std::endl;
+ //std::cout << "next qs in DEG \n" << (jointAngles+desq)* rl::math::RAD2DEG << std::endl;
// copy of code above to check for correctness
// forward kinematics for the right arm
diff --git a/python/preprocessing/genPaths.py b/python/preprocessing/genPaths.py
index 41f233cbb3e6f1496491cccdccdc504e9606cbbd..348f4ca70a0cc0c491f7631bae21df2f0d16bb1f 100644
--- a/python/preprocessing/genPaths.py
+++ b/python/preprocessing/genPaths.py
@@ -109,7 +109,7 @@ plt.show()
# number of points in paths
pNum = len(p1[:,0])
# length of wire, defined in 4-axis setup
-wLen = 0.8
+wLen = 0.4
z1 = np.zeros((pNum,1))
z2 = np.ones((pNum,1)) * wLen
# append z axis to position vectors
diff --git a/python/taskspace_placement/computeJoints_left.py b/python/taskspace_placement/computeJoints_left.py
index 8545332ba1452f1bdd29cec8f5d0a448d07a4d5c..0fab7b869175d3c474a8d149a2bb401d5cc258f0 100644
--- a/python/taskspace_placement/computeJoints_left.py
+++ b/python/taskspace_placement/computeJoints_left.py
@@ -41,16 +41,17 @@ 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, -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, 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
+#phi_const = np.array([100.0, 90.0, 120.0]) * np.pi/180.0
dphi_const = np.array([0.0, 0.0, 0.0]) * np.pi/180.0
phi_base = np.zeros((len(p1[:,0]),3)) + phi_const
@@ -68,8 +69,8 @@ for index, (pos, vel, phi, phi_dot) in enumerate(zip(p1, v1, phi_total, dphi)):
computedPose[index, :] = result[1]
if index > 0:
jointVelocities = (desJointAngles[index, :] - desJointAngles[index-1, :])/dt # only true in the ideal case where result of ik matches the desired pose
- print('IK joints:', result[0])
- print('IK resulting pose', result[1])
+ #print('IK joints:', result[0])
+ #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])