Eigen: Comma initialising dynamic matrix

Question

I am trying to write a software to compute the forward and inverse kinematics of a robotic arm with Eigen matrix library. I'm having a trouble initialising a comma-separated dynamic matrix. Im running into EXC_BAD_ACCESS LLDB debugger bug. I am new to LLDB debugger and not quite sure hot debug using it.

This is my main.cpp code and my class definition and implementation of class RobotArm seems to be fine.

#include <iostream>
#include <Eigen/Dense>
#include "RobotArm.h"

using namespace Eigen;
using namespace std;

int main(int argc, const char * argv[])
{
    RobotArm testArm;
    MatrixXd E(5,4);

    E << 0, 0, 0, 10,
         90, 30, 5, 0,
         0, 30, 25, 0,
         0, 30, 25, 0,
         0, 30, 0, 0;

    Vector3d POS = testArm.forwardKinematics(E);
    cout << "Position vector [X Y Z]" << POS << endl;
    return 0;
}

This is my RobotArm.h

#ifndef ____RobotArm__
#define ____RobotArm__

#include <stdio.h>
#include <Eigen/Dense>
#include <math.h>

using namespace std;
using namespace Eigen;

class RobotArm
{

public:
    //Constructor
    RobotArm();
    Vector3d forwardKinematics(MatrixXd DH);
    VectorXd inversekinematics();
    void homeArm();

private:
    double xPos, yPos, zPos;
    MatrixX4d T;
    MatrixX4d H;
    Vector3d pos;
    MatrixX4d substitute(double alpha, double theta, double a, double d);


};

#endif /* defined(____RobotArm__) */

This is my RobotArm.cpp

#include "RobotArm.h"
#include <stdio.h>
#include <Eigen/Dense>
#include <cmath>
#include <iostream>

using namespace std;
using namespace Eigen;


RobotArm::RobotArm()
{
    cout << "Successfully created RobotArm object" << endl;
}

Vector3d RobotArm::forwardKinematics(MatrixXd DH)
{
   MatrixX4d H;
   //H = MatrixX4d::Constant(4,4,1);
   H << 1,1,1,1,
        1,1,1,1,
        1,1,1,1,
        1,1,1,1;


    //Update current link parameters
    for (int i = 0; i < 6; i++)
    {
        H *= substitute(DH(i,0), DH(i,1), DH(i,2), DH(i,3));
    }

    pos(0,0) = H(0,3);
    pos(1,0) = H(1,3);
    pos(1,0) = H(2,3);

    return pos;
}

MatrixX4d RobotArm::substitute(double alpha, double theta, double a, double d)
{
    T << cos(theta), -sin(theta), 0, a,
         (sin(theta)*cos(alpha)), (cos(theta)*cos(alpha)), -sin(alpha), (-sin(alpha)*d),
         (sin(theta)*sin(alpha)),(cos(theta)*sin(alpha)), cos(alpha), (cos(alpha)*d),
         0, 0, 0, 1;

    return T;
}

The error seems to occur when trying initialise matrix E on main.cpp

Note: The software is still under development. What I have posted is just for testing my class. Please advice on how to learn to use LLDB debugger. Thank you.

Answer 1

Actually, your problem is in RobotArm.h and RobotArm.cpp. MatrixX4d is a half-dynamic matrix. What you want is Matrix4d , which is a statically sized 4x4 matrix. With the MatrixX4d type, the size before calling operator<< is 0x4, so trying to assign any values is giving you an access violation.

If you must use a MatrixX4d , then make sure to resize the matrix before using it, eg:

Eigen::MatrixX4d H;
H.resize(whateverSizeYouWantTheOtherDimensionToBe, Eigen::NoChange);