Trying to implement an absolute pitch constraint using computeEulerAngles()

[james-p-foster]

Hi,

I'm struggling to implement a constraint that enforces the absolute pitch and roll (that is, pitch and roll w.r.t. the inertial world frame) of the feet of my robot to be zero.

To me, the most natural way seems to be to define the contact coordinate frames on each foot, then use computeEulerAngles() on these frames to find the orientation of the foot relative to the world frame, and then put that angle into a SymFunction that is constrained to be zero.

When I do this, and compile, I get some kind of compilation/mex error, that the function "Dot" (which I'm assuming is originally from Mathematica) is not within scope:

Error using mex
/home/james/slider-hzd/slider2dof/gen/export/swingFootPitch_RightStance.cc: In function ‘void output1(double*, const
double*)’:
/home/james/slider-hzd/slider2dof/gen/export/swingFootPitch_RightStance.cc:114:119: error: ‘Dot’ was not declared in this
scope
   p_output1[0]=ArcTan(t448,Dot(t441*t443 - 1.*t405*t446,t448/Sqrt(Power(t417*t429*t431 + t428*t436,2) + Power(t448,2))));
                                                                                                                       ^


Error in build_mex (line 68)
        mex(...

Error in SymExpression/export (line 105)
        build_mex(export_path,filename);

Error in SymFunction/export (line 71)
    f = obj.export@SymExpression(export_opts);

Error in TrajectoryOptimization/compileConstraint>@(x)export(x.SymFun,export_path,opts)

Error in TrajectoryOptimization/compileConstraint (line 58)
        arrayfun(@(x)export(x.SymFun, export_path, opts), deps_array, 'UniformOutput', false);

Error in HybridTrajectoryOptimization/compileConstraint (line 33)
        compileConstraint(obj.Phase(k), constr, export_path, exclude, varargin{:});

Error in main (line 65)
nlp.compileConstraint([],[],EXPORT_PATH); 

For reference, here's my MATLAB function for creating a SymFunction of absolute foot pitch (absolute roll is nearly identical):

function [ swingFootPitchFun ] = swingFootPitch(nlp)

% Choose frames
switch nlp.Plant.Name
    case 'RightStance'
        swingFoot = nlp.Plant.ContactPoints.LeftFootBottom;
    case 'LeftStance'
        swingFoot = nlp.Plant.ContactPoints.RightFootBottom;
    otherwise
        error('Cannot create swingFootPitch constraint. Unknown domain.');
end

eulerAngles = swingFoot.computeEulerAngle();
swingFootPitchFun = SymFunction(['swingFootPitch_',nlp.Plant.Name], eulerAngles(2), {nlp.Plant.States.x});

end

As you can see the robot has a ContactPoints field where the contact frames are defined.

I've tried to dig into both the FROST file template.cc and my own Mathematica copy of mdefs.h in order to add functionality to the "Dot" function, but that opens a whole can of worms I don't know how to deal with.

Can anyone help me to either troubleshoot this Euler Angles approach, or if you have implemented a similar constraint, suggest an alternative method?

Cheers,

-- James

[weicheng9933]

May I ask how you solved this error problem, thanks!

[james-p-foster]

May I ask how you solved this error problem, thanks!

Apologies, as this was several years ago I've long since stopped working on this project. Best of luck!