The evaluation of Mathematica expression failed.

[zapplelove]

Dear ayonga: I have a robot urdf file, and I want to calculate the dynamics of it. First I tried to follow the cassie example and write my own calculation script file, but when I write Holomomic Constraints I have some trouble. In fact, my robot has closed-chain structures in it. So, I want to add some constraints to it. Here is my calculation script file:

        function obj = Cheetah(urdf)
            config = struct();
            config_file = GetFullPath(urdf);
            [config.name, config.links, config.joints, config.transmissions] = ros_load_urdf(config_file);
            obj = obj@RobotLinks(config, [], [], 'removeFixedJoints', true);
            obj.ConfigFile = config_file;
            
            %% Define contact frames
%             R = [0,-sin(deg2rad(50)),-cos(deg2rad(50));
%             0,cos(deg2rad(50)),-sin(deg2rad(50));
%             1,0,0];
                 
            % Bottom of L4
            FL_foot_frame = obj.Joints(getJointIndices(obj, 'FL1_4'));
            obj.ContactPoints.FL_foot_contact = CoordinateFrame(...
                'Name','FL_foot_contact',...
                'Reference',FL_foot_frame,...
                'Offset',[0.0177, 0.0522, 0],...
                'R',R...
                );
......
......
......
           %% Define other frames(IMU)
            Body_frame = obj.Joints(getJointIndices(obj, 'bar_B'));
                        
            IMU_frame = config.joints(strcmp({config.joints.Child}, 'IMU'));
            if ~isempty(IMU_frame)
                obj.OtherPoints.IMU = CoordinateFrame(...
                    'Name','IMU',...
                    'Reference',Body_frame,...
                    'Offset',IMU_frame.Offset,...
                    'R',IMU_frame.R...
                    );
            end
            %% Define Holomomic Constraints of loop chains
            % Spine loops(This is a triangular structure)
            FSb_l_var=(0.28561227+obj.States.x('FSb_l'));
            RSb_l_var=(0.28561227+obj.States.x('RSb_l'));
            B_S = [acos((0.20849221^2+0.16530351^2-FSb_l_var.*FSb_l_var)/(2*0.20849221*0.16530351));
                acos((0.20849221^2+0.16530351^2-RSb_l_var.*RSb_l_var)/(2*0.20849221*0.16530351))]-...
                [1.72783556;1.72783556]-[obj.States.x('B_FS');obj.States.x('B_RS')];
            B_Sb = [acos((0.20849221^2+FSb_l_var.*FSb_l_var-0.16530351^2)/(2*0.20849221*FSb_l_var));
                acos((0.20849221^2+RSb_l_var.*RSb_l_var-0.16530351^2)/(2*0.20849221*RSb_l_var))]-...
                [0.60851175;0.60851175]-[obj.States.x('B_FSb');obj.States.x('B_RSb')];
            B_S_constr = HolonomicConstraint(obj, B_S, 'B_S', ...
                'ConstrLabel', {{'B_S_F', 'B_S_R'}}, ...
                'Jacobian', jacobian(B_S, obj.States.x));
            B_Sb_constr = HolonomicConstraint(obj, B_Sb, 'B_Sb', ...
                'ConstrLabel', {{'B_Sb_F', 'B_Sb_R'}}, ...
                'Jacobian', jacobian(B_Sb, obj.States.x));
            obj = addHolonomicConstraint(obj, B_S_constr);
            obj = addHolonomicConstraint(obj, B_Sb_constr);
            
            % leg loops
        end

And here is the error message:

ans =

    '12.1.0 for Microsoft Windows (64-bit) (March 14, 2020)'

73              B_S = [acos((0.20849221^2+0.16530351^2-FSb_l_var.*FSb_l_var)/(2*0.20849221*0.16530351));
Thread::tdlen: Objects of unequal length in 
                                                               2
    {{-1.72784 + ArcCos[14.5077 (0.070794 - (0.285612 + x$32$1) )]}, 
                                                                2
      {-1.72784 + ArcCos[14.5077 (0.070794 - (0.285612 + x$33$1) )]}} + 
     {{-x$4$1}, {-x$30$1}, {-x$17$1}, {-x$31$1}} cannot be combined.
error using eval_math (line 46)
The evaluation of Mathematica expression failed.

error in SymExpression (line 91)
                    eval_math([obj.s '=' obj.f ';']);

error in  -  (line 266)
            X = SymExpression(sstr);

error in Cheetah (line 73)
            B_S = [acos((0.20849221^2+0.16530351^2-FSb_l_var.*FSb_l_var)/(2*0.20849221*0.16530351));

error in cheetah_gen (line 24)
cheetah = Cheetah('urdf/cheetah_frost.urdf');

What should I do to solve this? Looking forward to your reply. And any plan to change the background to matlab symbolic calculation?