You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
96 lines
1.8 KiB
96 lines
1.8 KiB
#ifndef _ROBOT_H_
|
|
#define _ROBOT_H_
|
|
|
|
#include "vec.h"
|
|
#include "mat.h"
|
|
#include "friComm.h"
|
|
|
|
typedef Mat<float, 4> MatCarthesian;
|
|
typedef Vec<float, FRI_CART_VEC> VecTorque;
|
|
|
|
class Robot
|
|
{
|
|
public :
|
|
const static int joints = 7;
|
|
typedef Vec<float,joints> VecJoint;
|
|
|
|
static int getJointCount(void)
|
|
{
|
|
}
|
|
static VecJoint getJointRange(void)
|
|
{
|
|
return VecJoint(180.0f);
|
|
}
|
|
static VecJoint getJointVelocity(void)
|
|
{
|
|
return VecJoint(200.0f);
|
|
}
|
|
static VecJoint getJointAcceleration(void)
|
|
{
|
|
return VecJoint(10.0f);
|
|
}
|
|
|
|
static Vec<float, 4> getDhParameter(unsigned int joint)
|
|
{
|
|
// unused parameter
|
|
(void) joint;
|
|
return Vec<float,4>(0.0f);
|
|
}
|
|
|
|
struct RobotConfig
|
|
{
|
|
bool elbow;
|
|
bool flip;
|
|
bool j1os;
|
|
};
|
|
|
|
static VecJoint backwardCalc(MatCarthesian, struct RobotConfig config );
|
|
static MatCarthesian forwardCalc(VecJoint, struct RobotConfig config );
|
|
};
|
|
|
|
|
|
|
|
#if 0
|
|
Mat4f vecToMat(float vec[12])
|
|
{
|
|
Mat4f result;
|
|
for (int i=0; i<3; i++) // ZEILE
|
|
{
|
|
for (int j=0; j<4; j++) // SPALTE
|
|
{
|
|
result(i,j) = vec[i*4+j];
|
|
}
|
|
}
|
|
result(3,3)= 1.0f;
|
|
return result;
|
|
}
|
|
|
|
float* matToVec(Mat4f mat)
|
|
{
|
|
float* vec = new float[12];
|
|
for (int j=0;j<3;j++)
|
|
{
|
|
for (int k=0;k<4;k++)
|
|
{
|
|
vec[j*4+k] = mat(j,k);
|
|
}
|
|
}
|
|
return vec;
|
|
}
|
|
|
|
Mat4f getTranslation(double tx, double ty, double tz)
|
|
{
|
|
Mat4f transl;
|
|
transl(0,0) = 1;
|
|
transl(1,1) = 1;
|
|
transl(2,2) = 1;
|
|
|
|
transl(0,3) = tx;
|
|
transl(1,3) = ty;
|
|
transl(2,3) = tz;
|
|
transl(3,3) = 1;
|
|
return transl;
|
|
}
|
|
#endif
|
|
|
|
#endif
|