idyntree
idyntree copied to clipboard
Published
20 hours ago •
robotology
robotology
iDynTree.Twist.Zero() does not return an iDynTree.Twist object in MATLAB bindings
I'm trying to use kinDynComputations with Matlab and I'm running into an error.
When I call kinDynComp.setRobotState I got the following error
standalone_test
[loadReducedModel]: loading the following model: /home/gromualdi/robot-code/robotology-superbuild/build/install/share/iCub/robots/iCubGazeboV3/model.urdf
[WARNING] SensorElement :: setAttributes : iDynTree does not support sensor of type depth
[WARNING] SensorElement :: setAttributes : iDynTree does not support sensor of type camera
[loadReducedModel]: loaded model: /home/gromualdi/robot-code/robotology-superbuild/build/install/share/iCub/robots/iCubGazeboV3/model.urdf, number of joints: 23
Error using iDynTreeMEX
No matching function for overload function
'KinDynComputations_setRobotState'. Possible C/C++ prototypes are:
iDynTree::KinDynComputations::setRobotState(iDynTree::Transform const
&,iDynTree::VectorDynSize const &,iDynTree::Twist const
&,iDynTree::VectorDynSize const &,iDynTree::Vector3 const &)
iDynTree::KinDynComputations::setRobotState(iDynTree::MatrixView<
double const >,iDynTree::Span< double const,-1 >,iDynTree::Span<
double const,-1 >,iDynTree::Span< double const,-1 >,iDynTree::Span<
double const,-1 >)
iDynTree::KinDynComputations::setRobotState(iDynTree::VectorDynSize
const &,iDynTree::VectorDynSize const &,iDynTree::Vector3 const &)
iDynTree::KinDynComputations::setRobotState(iDynTree::Span< double
const,-1 >,iDynTree::Span< double const,-1 >,iDynTree::Span< double
const,-1 >)
Error in iDynTree.KinDynComputations/setRobotState (line 72)
[varargout{1:nargout}] = iDynTreeMEX(1730, self, varargin{:});
Error in SwigRef/subsref (line 33)
[varargout{1:nargout}] = builtin('subsref',self,s);
Error in standalone_test (line 35)
model.kinDynComp.setRobotState(base_pose, jointPos_iDyntree, ...
This is the standalone script that you may run to replicate the problem standalone_test.zip
I hope this was not caused by https://github.com/robotology/idyntree/pull/736
Matlab users (@Giulero, @gabrielenava @HosameldinMohamed @traversaro) Do you have any hints?
I have a terrible doubt. Did you loaded casadi in the same workspace in which you are having this problem? If that is the case, can you try to avoid loading casadi and/or to have casadi at all in the MATLABPATH ?
This is what my matlabpath contains (I didn't copy the folders stored in matlabroot)
/home/gromualdi/Documents/MATLAB
/home/gromualdi/robot-code/robotology-superbuild/build/install/mex
/home/gromualdi/robot-code/robotology-superbuild/build/install/share/WBToolbox
/home/gromualdi/robot-code/robotology-superbuild/build/install/share/WBToolbox/images
gromualdi@iiticublap109:~/robot-code/robotology-superbuild/build/src/iDynTree (cppad)$ ctest -VV -R matlab
UpdateCTestConfiguration from :/home/gromualdi/robot-code/robotology-superbuild/build/src/iDynTree/DartConfiguration.tcl
Parse Config file:/home/gromualdi/robot-code/robotology-superbuild/build/src/iDynTree/DartConfiguration.tcl
UpdateCTestConfiguration from :/home/gromualdi/robot-code/robotology-superbuild/build/src/iDynTree/DartConfiguration.tcl
Parse Config file:/home/gromualdi/robot-code/robotology-superbuild/build/src/iDynTree/DartConfiguration.tcl
Test project /home/gromualdi/robot-code/robotology-superbuild/build/src/iDynTree
Constructing a list of tests
Done constructing a list of tests
Updating test list for fixtures
Added 0 tests to meet fixture requirements
Checking test dependency graph...
Checking test dependency graph end
test 61
Start 61: matlab_idyntree_tests
61: Test command: /home/gromualdi/matlabroot/bin/matlab "-nodisplay" "-nodesktop" "-nojvm" "-r" "addpath('/home/gromualdi/robot-code/robotology-superbuild/build/src/iDynTree/lib');addpath('/home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab/autogenerated');addpath('/home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab');addpath('/home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab/tests/');addpath(genpath('/home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/extern/MOxUnit/MOxUnit'));success=moxunit_runtests('/home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab/tests','-verbose');exit(~success);"
61: Test timeout computed to be: 1500
61:
61: < M A T L A B (R) >
61: Copyright 1984-2020 The MathWorks, Inc.
61: R2020b Update 4 (9.9.0.1570001) 64-bit (glnxa64)
61: January 7, 2021
61:
61: Warning: Name is nonexistent or not a directory:
61: /home/gromualdi/robot-code/robotology-superbuild/build/install/mex/+wbc/simulink
61:
61: For online documentation, see https://www.mathworks.com/support
61: For product information, visit www.mathworks.com.
61:
61:
61: Non-Degree Granting Education License -- for use at non-degree granting, nonprofit,
61: educational organizations only. Not for government, commercial, or other organizational use.
61:
61: suite: 0 tests
61:
61: --------------------------------------------------
61:
61: OK
61:
1/1 Test #61: matlab_idyntree_tests ............ Passed 3.43 sec
The following tests passed:
matlab_idyntree_tests
100% tests passed, 0 tests failed out of 1
For an additional data point, it would be interesting to run the test suite also for octave:
sudo apt install liboctave-dev
// Enable ROBOTOLOGY_USES_OCTAVE or IDYNTREE_USES_OCTAVE
ctest -VV -R octave
Cool, at some point the test suites stopped reading any test, and that is the reason why is passing also for octave, because zero tests were found and zero are passing. -_-
ctest -VV -R octave
UpdateCTestConfiguration from :/home/gromualdi/robot-code/robotology-superbuild/build/src/iDynTree/DartConfiguration.tcl
Parse Config file:/home/gromualdi/robot-code/robotology-superbuild/build/src/iDynTree/DartConfiguration.tcl
UpdateCTestConfiguration from :/home/gromualdi/robot-code/robotology-superbuild/build/src/iDynTree/DartConfiguration.tcl
Parse Config file:/home/gromualdi/robot-code/robotology-superbuild/build/src/iDynTree/DartConfiguration.tcl
Test project /home/gromualdi/robot-code/robotology-superbuild/build/src/iDynTree
Constructing a list of tests
Done constructing a list of tests
Updating test list for fixtures
Added 0 tests to meet fixture requirements
Checking test dependency graph...
Checking test dependency graph end
test 62
Start 62: octave_idyntree_tests
62: Test command: /usr/bin/octave "--no-gui" "--quiet" "--eval" "addpath('/home/gromualdi/robot-code/robotology-superbuild/build/src/iDynTree/lib');addpath('/home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab/autogenerated');addpath('/home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab');addpath('/home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab/tests/');addpath(genpath('/home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/extern/MOxUnit/MOxUnit'));success=moxunit_runtests('/home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab/tests','-verbose');exit(~success);"
62: Test timeout computed to be: 1500
62: warning: isdir is obsolete; use isfolder or dir_in_loadpath instead
62: suite: 8 tests
62: passed: test_span: /home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab/tests/EKFTest.m
62: passed: test_span_qekf: /home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab/tests/EKFTest.m
62: passed: test_momentum_invariance: /home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab/tests/InertiaUnitTest.m
62: passed: test_joint_constructor: /home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab/tests/JointUnitTest.m
62: warning: iDynTreeMEX: Resizing iDynTree vector to (10,20)
62: passed: test_sparse_matrices: /home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab/tests/MatrixUnitTest.m
62: passed: test_sum_of_positions: /home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab/tests/PositionUnitTest.m
62: passed: test_pos_twist_wrench_invariance: /home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab/tests/TransformUnitTest.m
62: 1/28: testing loadReducedModel...
62: [loadReducedModel]: loading the following model: ./model.urdf
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [WARNING] MaterialElement :: childElementForName::texture : Texture tag not supported by iDynTree. Skipping material material.metal
62: [loadReducedModel]: loaded model: ./model.urdf, number of joints: 25
62: 2/28: testing setRobotState...
62: [setRobotState]: debugging inputs...
62: [setRobotState]: done.
62: 3/28: testing setJointPos...
62: [setJointPos]: debugging inputs...
62: [setJointPos]: done.
62: 4/28: testing setFrameVelocityRepresentation...
62: 5/28: testing setFloatingBase...
62: 6/28: testing getJointPos...
62: 7/28: testing getJointVel...
62: 8/28: testing getCentroidalTotalMomentum...
62: 9/28: testing getNrOfDegreesOfFreedom...
62: [getNrOfDegreesOfFreedom]: debugging outputs...
62: [getNrOfDegreesOfFreedom]: done.
62: 10/28: testing getCenterOfMassPosition...
62: 11/28: testing getBaseTwist...
62: 12/28: testing generalizedBiasForces...
62: 13/28: testing generalizedGravityForces...
62: 14/28: testing getWorldBaseTransform...
62: [getWorldBaseTransform]: debugging outputs...
62: [getWorldBaseTransform]: done.
62: 15/28: testing getModelVel...
62: 16/28: testing getFrameVelocityRepresentation...
62: 17/28: testing getFloatingBase...
62: 18/28: testing getFrameIndex...
62: 19/28: testing getFrameName...
62: 20/28: testing getWorldTransform...
62: [getWorldTransform]: debugging outputs...
62: [getWorldTransform]: done.
62: 21/28: testing getRelativeTransform...
62: [getRelativeTransform]: debugging outputs...
62: [getRelativeTransform]: done.
62: 22/28: testing getRelativeJacobian...
62: 23/28: testing getFreeFloatingMassMatrix...
62: [getFreeFloatingMassMatrix]: debugging outputs...
62: [getFreeFloatingMassMatrix]: done.
62: 24/28: testing getRobotState...
62: [getRobotState]: debugging outputs...
62: [getRobotState]: done.
62: 25/28: testing getFrameBiasAcc...
62: 26/28: testing getCenterOfMassJacobian...
62: 27/28: testing getCenterOfMassVelocity...
62: 28/28: testing getFrameFreeFloatingJacobian...
62: passed: test__high_level_wrappers: /home/gromualdi/robot-code/robotology-superbuild/src/iDynTree/bindings/matlab/tests/highLevelWrappersSmokeTest.m
62:
62: --------------------------------------------------
62:
62: OK
1/1 Test #62: octave_idyntree_tests ............ Passed 0.56 sec
The following tests passed:
octave_idyntree_tests
100% tests passed, 0 tests failed out of 1
Total Test time (real) = 0.57 sec
Hi @GiulioRomualdi I don't know a solution. If this helps. I was able to replicate the same error you got by running the script. My iDynTree is at the commit bf8d0f3ee6862ca4bcd6fb76eafefd0539574df6
I tried to run the script you added in the above comment and I got the same error. Curiously, I tried to call setRobotState via the iDynTreeWrappers and I got a different error:
Unrecognized method, property, or field 'paren' for class 'iDynTree.Transform'.
Error in SwigRef/subsref (line 28)
[varargout{1:nargout}] =
builtin('subsref',self,substruct('.','paren','()',s.subs));
Error in iDynTreeWrappers.setRobotState (line 145)
baseOrigin_iDyntree.setVal(k,basePose(k+1,4));
Error in standalone_test (line 39)
iDynTreeWrappers.setRobotState(model,base_pose,jointPos_iDyntree,base_velocity,jointVel_iDyntree,gravity);
>>
here is the modification I did in the code:
disp("testing idyntree wrappers...")
iDynTreeWrappers.setRobotState(model,base_pose,jointPos_iDyntree,base_velocity,jointVel_iDyntree,gravity);
disp("passed")
substituting
base_velocity = iDynTree.Twist.Zero();
with fixes the problem
base_velocity = iDynTree.Twist();
base_velocity.zero();
I am not sure why, but this piece of script runs with no error. I got it from the examples with modifications if I remember well.
The difference here is that KinDynComputations is created as an object then loaded with the model, in contrast with what you wrote.
Also base_velocity = iDynTree.Twist.Zero(); seems fine here!
%% In this example, we use the iDynTree.Model and the iDynTree.KinDynComputations
% classes to express the joint axis for a set of revolute axis in the base
% frame
% Create KinDynComputations class
comp = iDynTree.KinDynComputations();
% Load model from an URDF file
loader = iDynTree.ModelLoader
loader.loadModelFromFile('model.urdf')
comp.loadRobotModel(loader.model)
% Check if the model was correctly created by printing the model
comp.getRobotModel().toString()
comp.getRobotModel().getTotalMass()
%% Set kinematics information
% For computing the forward kinematics, we need to set the robot position
% and velocity with respect to an inertial frame and the joint positions
% and velocities. In this case we will just put the joint in the 0 position
% and the world_H_base to identity and all the velocities to 0
robotState = {};
robotState.world_H_base = iDynTree.Transform.Identity();
robotState.base_vel = iDynTree.Twist.Zero();
% The joint pos/vel vectors are automatically resize with a constructor
% that takes in input the robot model
robotState.qj = iDynTree.VectorDynSize();
robotState.qj.resize(comp.getRobotModel().getNrOfPosCoords());
robotState.qj.zero();
robotState.dqj = iDynTree.VectorDynSize();
robotState.dqj.resize(comp.getRobotModel().getNrOfDOFs());
robotState.dqj.zero();
% Gravity expressed in the world frame
robotState.gravity = iDynTree.Vector3();
robotState.gravity.fromMatlab([0,0,-9.81]);
% Actually set the state in the kinDynComp class (we use the setted that
% does not requires to set the base variables and assume that they are
% the identity and zero, because the full one for some reason does not
% works)
% comp.setRobotState(robotState.world_H_base,robotState.qj,robotState.base_vel,robotState.dqj,robotState.gravity);
comp.setRobotState(robotState.qj,robotState.dqj,robotState.gravity);
No nevermind! it's a different function!
The problems seems to be that iDynTree.Twist.Zero() return SpatialVector<SpatialMotionVector> that is a CRTP-defined type from which Twist is derived, but that probably is not correctly understood by SWIG, and this result in this behaviour that clearly violates the POLA . Since we remove the semantic support probably all the CRTP machiner of SpatialVector is not so necessary and in the future we can remove it, but in the meanwhile probably just defining:
Twist Twist::Zero() {
return Twist(SpatialVector<SpatialMotionVector>::Zero())`
}
will probably do the trick and fix the problem.