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.
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, e.g.:
Eigen::MatrixX4d H;
H.resize(whateverSizeYouWantTheOtherDimensionToBe, Eigen::NoChange);