stephane-caron/analytical-wrench-cone

Analytical formula of the wrench friction cone for surface contacts

analytical-wrench-cone's Issues

Contact wrench transformation question

Hi Stephane,
Thank you very much for providing the accompanying code to your paper.

I had a question on your contact wrench transformation, specifically in the following line:
https://github.com/stephane-caron/analytical-wrench-cone/blob/master/dmotions/hrp4/topp/wrench_cons.py#L187

My thought is that if my wrenches are defined in the world frame, then I need to use the Adjoint operator [1] to properly transform my world frame wrench to a local frame representation. The Adjoint operator would be a 6x6 transformation matrix that contains the rotation matrix R along the block diagonal, and an off-diagonal block matrix to account for local frame torques due to world frame linear forces. However, your implementation only contains the rotation matrix R along the diagonal.

For instance, I was expecting the transformation to be similar to the following pseudocode:

% F_a = [m_a, f_a] is a 6x1 vector representing the contact wrench in frame a (world), 
%    where m_a are moments about axes of frame a, and f_a are linear force vectors 
%    described in frame a.
% F_b = [m_b, f_b] is a 6x1 vector representing the contact wrench in frame b (local),
%    where m_b are moments about axes of frame b, and f_b are linear force vectors 
%    described in frame b.
% R is a 3x3 rotation matrix describing the orientation of frame b wr.t. to a
% p is 3x1 vector describing the linear offset from frame a to b.
% [p] is the 3x3 skew symmetric matrix of p. 

Adj_{a,b}(R,p) = [R, 0; [p]R, R] % The 6x6 Adjoint transformation matrix
F_b = Adj_{a,b}(R,p)^T * F_a % The resulting 6x1 local frame wrench representation

Am I misunderstanding your intended transformation? Otherwise what I am I missing? I would greatly appreciate your thoughts on this matter.

Thank you!

[1] Lynch, Kevin M., and Frank C. Park. Modern Robotics. Cambridge University Press, 2017. pp. 110, equation 3.95. Available Online: PDF

[TOPP::ComputeProfiles] failed: CLC failed

Hi, Carson:
From generate_motion.py, the following error messages appeared:

[TOPP] (a, b, c) computation: 5.65 s
IntegrateForward INT_BOTTOM, s=5.689403454912973e+00, sd=5.608232237136747e-01
CLC failed
[TOPP::ComputeProfiles] failed: CLC failed
Traceback (most recent call last):
File "./generate_motion.py", line 366, in
retimed_traj = hrp.topp.retime_trajectory(original_traj, 0., 0.)
File "....../dmotions/hrp4/topp/retimer.py", line 88, in retime_trajectory
topp_traj = topp_inst.Reparameterize(sd_beg, sd_end)
File "/usr/local/lib/python2.7/dist-packages/TOPP/QuadraticConstraints.py", line 50, in Reparameterize
raise NoTrajectoryFound(return_code)
TOPP.Errors.NoTrajectoryFound: unspecified error

Would you like to give me some advice?
Thank you very much.