You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
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
The text was updated successfully, but these errors were encountered:
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:
For reference, here's my MATLAB function for creating a SymFunction of absolute foot pitch (absolute roll is nearly identical):
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
The text was updated successfully, but these errors were encountered: