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
...@@ -11,13 +11,14 @@ import ur5_reaching_conf as conf ...@@ -11,13 +11,14 @@ import ur5_reaching_conf as conf
print(("".center(conf.LINE_WIDTH,'#'))) print(("".center(conf.LINE_WIDTH,'#')))
print((" TSID - Manipulator End-Effector Sin Tracking ".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_POS = 1
PLOT_EE_VEL = 1 PLOT_EE_VEL = 0
PLOT_EE_ACC = 0 PLOT_EE_ACC = 0
PLOT_JOINT_VEL = 1 PLOT_JOINT_VEL = 0
PLOT_TORQUES = 1 PLOT_TORQUES = 0
tsid = TsidManipulator(conf) tsid = TsidManipulator(conf)
...@@ -37,6 +38,7 @@ sampleEE = tsid.trajEE.computeNext() ...@@ -37,6 +38,7 @@ sampleEE = tsid.trajEE.computeNext()
samplePosture = tsid.trajPosture.computeNext() samplePosture = tsid.trajPosture.computeNext()
offset = sampleEE.pos() offset = sampleEE.pos()
offset[3:] = np.array([1., 0, 0, 0., 1, 0, 0., 0, 1]) # useless
offset[:3] += conf.offset offset[:3] += conf.offset
two_pi_f_amp = conf.two_pi_f * conf.amp two_pi_f_amp = conf.two_pi_f * conf.amp
two_pi_f_squared_amp = conf.two_pi_f * two_pi_f_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 ...@@ -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 amp = np.array([0.0, 0.0, 0.0]) # amplitude
phi = np.array([0.0, 0.5*np.pi, 0.0]) # phase 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) 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_ee = 1.0 # weight of end-effector task
w_posture = 1e-3 # weight of joint posture task w_posture = 1e-3 # weight of joint posture task
......
...@@ -49,7 +49,10 @@ namespace tsid ...@@ -49,7 +49,10 @@ namespace tsid
const pinocchio::SE3 & Mdes, const pinocchio::SE3 & Mdes,
pinocchio::Motion & error) 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, void solveWithDampingFromSvd(Eigen::JacobiSVD<Eigen::MatrixXd> & svd,
......
...@@ -23,6 +23,7 @@ namespace tsid ...@@ -23,6 +23,7 @@ namespace tsid
{ {
namespace tasks namespace tasks
{ {
using namespace std;
using namespace math; using namespace math;
using namespace trajectories; using namespace trajectories;
using namespace pinocchio; using namespace pinocchio;
...@@ -181,10 +182,9 @@ namespace tsid ...@@ -181,10 +182,9 @@ namespace tsid
SE3ToVector(m_M_ref, m_p_ref); SE3ToVector(m_M_ref, m_p_ref);
SE3ToVector(oMi, m_p); SE3ToVector(oMi, m_p);
if (m_local_frame) {
// Transformation from local to world // Transformation from local to world
m_wMl.rotation(oMi.rotation()); m_wMl.rotation(oMi.rotation());
if (m_local_frame) {
m_v_error = v_frame - m_wMl.actInv(m_v_ref); // vel err in local frame m_v_error = v_frame - m_wMl.actInv(m_v_ref); // vel err in local frame
// desired acc in local frame // desired acc in local frame
...@@ -192,8 +192,13 @@ namespace tsid ...@@ -192,8 +192,13 @@ namespace tsid
- m_Kd.cwiseProduct(m_v_error.toVector()) - m_Kd.cwiseProduct(m_v_error.toVector())
+ m_wMl.actInv(m_a_ref).toVector(); + m_wMl.actInv(m_a_ref).toVector();
} else { } 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_vec = m_wMl.toActionMatrix() * // pos err in local world-oriented frame
m_p_error.toVector(); 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_v_error = m_wMl.act(v_frame) - m_v_ref; // vel err in local world-oriented frame
m_drift = m_wMl.act(m_drift); m_drift = m_wMl.act(m_drift);
...@@ -225,6 +230,7 @@ namespace tsid ...@@ -225,6 +230,7 @@ namespace tsid
idx += 1; idx += 1;
} }
return m_constraint; return m_constraint;
} }
} }
......
Supports Markdown
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