Commit 5cd270b3 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

[c++] Fix bug in TaskSE3Equality: when expressing task quantities in world...

[c++] Fix bug in TaskSE3Equality: when expressing task quantities in world frame there was a mistake in the transform of the position error from local to world frame. Switch from log6 to log3 to avoid coupling between orientation error and translation error, which made task mask not work properly.
parent 6d1436ac
Pipeline #12634 passed with stage
in 22 minutes and 21 seconds
......@@ -11,13 +11,14 @@ import ur5_reaching_conf as conf
print(("".center(conf.LINE_WIDTH,'#')))
print((" TSID - Manipulator End-Effector Sin Tracking ".center(conf.LINE_WIDTH, '#')))
print(("".center(conf.LINE_WIDTH,'#'), '\n'))
print(("".center(conf.LINE_WIDTH,'#')))
print("")
PLOT_EE_POS = 1
PLOT_EE_VEL = 1
PLOT_EE_VEL = 0
PLOT_EE_ACC = 0
PLOT_JOINT_VEL = 1
PLOT_TORQUES = 1
PLOT_JOINT_VEL = 0
PLOT_TORQUES = 0
tsid = TsidManipulator(conf)
......@@ -37,6 +38,7 @@ sampleEE = tsid.trajEE.computeNext()
samplePosture = tsid.trajPosture.computeNext()
offset = sampleEE.pos()
offset[3:] = np.array([1., 0, 0, 0., 1, 0, 0., 0, 1]) # useless
offset[:3] += conf.offset
two_pi_f_amp = conf.two_pi_f * conf.amp
two_pi_f_squared_amp = conf.two_pi_f * two_pi_f_amp
......
......@@ -19,7 +19,7 @@ q0 = np.array([ 0. , -1.0, 0.7, 0. , 0. , 0. ]) # initial configuration
amp = np.array([0.0, 0.0, 0.0]) # amplitude
phi = np.array([0.0, 0.5*np.pi, 0.0]) # phase
two_pi_f = 2*np.pi*np.array([1.0, 0.5, 0.5]) # frequency (time 2 PI)
offset = np.array([0.2, 0.0, 0.0])
offset = np.array([0.1, 0.0, 0.0])
w_ee = 1.0 # weight of end-effector task
w_posture = 1e-3 # weight of joint posture task
......
......@@ -49,7 +49,10 @@ namespace tsid
const pinocchio::SE3 & Mdes,
pinocchio::Motion & error)
{
error = pinocchio::log6(Mdes.inverse() * M);
// error = pinocchio::log6(Mdes.inverse() * M);
pinocchio::SE3 M_err = Mdes.inverse() * M;
error.linear() = M_err.translation();
error.angular() = pinocchio::log3(M_err.rotation());
}
void solveWithDampingFromSvd(Eigen::JacobiSVD<Eigen::MatrixXd> & svd,
......
......@@ -23,6 +23,7 @@ namespace tsid
{
namespace tasks
{
using namespace std;
using namespace math;
using namespace trajectories;
using namespace pinocchio;
......@@ -181,10 +182,9 @@ namespace tsid
SE3ToVector(m_M_ref, m_p_ref);
SE3ToVector(oMi, m_p);
// Transformation from local to world
m_wMl.rotation(oMi.rotation());
if (m_local_frame) {
// Transformation from local to world
m_wMl.rotation(oMi.rotation());
m_v_error = v_frame - m_wMl.actInv(m_v_ref); // vel err in local frame
// desired acc in local frame
......@@ -192,8 +192,13 @@ namespace tsid
- m_Kd.cwiseProduct(m_v_error.toVector())
+ m_wMl.actInv(m_a_ref).toVector();
} else {
// Transformation from reference to world
m_wMl.rotation(m_M_ref.rotation());
m_p_error_vec = m_wMl.toActionMatrix() * // pos err in local world-oriented frame
m_p_error.toVector();
// Transformation from local to world
m_wMl.rotation(oMi.rotation());
m_v_error = m_wMl.act(v_frame) - m_v_ref; // vel err in local world-oriented frame
m_drift = m_wMl.act(m_drift);
......@@ -225,6 +230,7 @@ namespace tsid
idx += 1;
}
return m_constraint;
}
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment