ComauSmartSix::ComauSmartSix(
std::string robot_description,
std::string base_link,
std::string tip_link
) {
this->robot_description = robot_description;
this->pose_temp_data = new float[7];
/** Create TREE */
if (!kdl_parser::treeFromString(robot_description, this->tree)) {
ROS_ERROR("Failed to construct kdl tree");
} else {
this->tree_solver_fk = new TreeFkSolverPos_recursive(this->tree);
/** Create CHAIN */
this->tree.getChain( base_link, tip_link, this->chain );
chain_solver_fk = new ChainFkSolverPos_recursive(this->chain);
chain_solver_vel_ik = new ChainIkSolverVel_pinv(this->chain);
/** Joints LIMITS */
int n = this->tree.getNrOfJoints();
ROS_INFO("Instantiated new Robot with %d joints",n);
this->q_limit_max = JntArray(n);
this->q_limit_min = JntArray(n);
/** TODO: pass limits with Params */
q_limit_max(0)=2.9670;
q_limit_max(1)=2.7052;
q_limit_max(2)=0;
q_limit_max(3)=3.6651; //3.14;//3.6651;
q_limit_max(4)=2.2689; //1.7444;//2.2689;
q_limit_max(5)=6.28; //47.1;
q_limit_min(0)=-2.9670;
q_limit_min(1)=-1.4835;
q_limit_min(2)=-2.9670;
q_limit_min(3)=-3.6651; //-3.14;//-3.6651;
q_limit_min(4)=-2.2689; //-1.7444;//-2.2689;
q_limit_min(5)=-6.28; //-47.1;
/** Refine Sovlers */
chain_solver_ik_jl = new ChainIkSolverPos_NR_JL(
this->chain,
q_limit_min,
q_limit_max,
*chain_solver_fk,
*chain_solver_vel_ik
);
chain_solver_ik_lma = new ChainIkSolverPos_LMA(this->chain);
/** Tool */
this->tool.p = {0,0,0};
this->tool.M = KDL::Rotation::EulerZYZ(0, 0, 0);
}
}
ChainIkSolverVel_pinv::ChainIkSolverVel_pinv(const Chain& _chain,double _eps,int _maxiter):
chain(_chain),
jnt2jac(chain),
jac(chain.getNrOfJoints()),
svd(jac),
U(6,JntArray(chain.getNrOfJoints())),
S(chain.getNrOfJoints()),
V(chain.getNrOfJoints(),JntArray(chain.getNrOfJoints())),
tmp(chain.getNrOfJoints()),
eps(_eps),
maxiter(_maxiter),
nrZeroSigmas(0),
svdResult(0)
{
}
ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, double _eps, int _maxiter, double _alpha):
chain(_chain),
jnt2jac(chain),
jac(chain.getNrOfJoints()),
svd(jac),
U(6,JntArray(chain.getNrOfJoints())),
S(chain.getNrOfJoints()),
V(chain.getNrOfJoints(),JntArray(chain.getNrOfJoints())),
tmp(chain.getNrOfJoints()),
tmp2(chain.getNrOfJoints()-6),
eps(_eps),
maxiter(_maxiter),
alpha(_alpha)
{
}
augmented_solver::augmented_solver(const Chain& _chain,double _eps,int _maxiter):
chain(_chain),
jnt2jac(chain),
jac(chain.getNrOfJoints()),
svd(jac),
U(6,JntArray(chain.getNrOfJoints())),
S(chain.getNrOfJoints()),
V(chain.getNrOfJoints(),JntArray(chain.getNrOfJoints())),
tmp(chain.getNrOfJoints()),
eps(_eps),
maxiter(_maxiter)
{
base_is_actived_ = true;
base_to_arm_factor_ = 0.1;
}
int ComauSmartSix::fk(float* q_in,float& x,float& y,float& z,float& e1, float& e2, float& e3) {
KDL::Frame pose;
int n = this->tree.getNrOfJoints();
KDL::JntArray j_q_in = JntArray(n);
for(int i = 0; i < n; i++)
j_q_in(i)=q_in[i];
this->chain_solver_fk->JntToCart(j_q_in,pose);
pose = pose * this->tool;
x = pose.p[0];
y = pose.p[1];
z = pose.p[2];
double roll,pitch,yaw;
pose.M.GetRPY(roll,pitch,yaw);
e1 = roll;
e2 = pitch;
e3 = yaw;
}
int ComauSmartSix::ik(float x, float y,float z,float roll, float pitch,float yaw,float* q_in,float* q_out,bool use_radians ) {
//TOOL
/*KDL::Frame tool;
tool.p = {0,0,-0.095f};
tool.M = KDL::Rotation::EulerZYZ(0 , PI, 0);*/
//KDL::Frame tool;
// tool.p = {-0.045f,0.0725f,-0.253f};
// tool.M = KDL::Rotation::EulerZYZ(0 , PI, 0);
KDL::Frame cartesian;
cartesian.p = {
x/1000.0f,
y/1000.0f,
z/1000.0f
};
if(!use_radians) {
roll = roll*PI/180.0f;
pitch = pitch*PI/180.0f;
yaw = yaw*PI/180.0f;
}
cartesian.M = KDL::Rotation::RPY(roll, pitch, yaw);
cartesian = cartesian * this->tool.Inverse();
unsigned int n = this->tree.getNrOfJoints();
KDL::JntArray j_q_in = JntArray(n);
KDL::JntArray j_q_out = JntArray(n);
for(int i = 0; i < n; i++)
j_q_in(i)=q_in[i];
int c = chain_solver_ik_jl->CartToJnt( j_q_in,cartesian,j_q_out );
for(int i = 0; i < n; i++)
q_out[i] = j_q_out(i);
return c;
}
int ComauSmartSix::fk(float* q_in,float& x,float& y,float& z,float& qx, float& qy, float& qz,float& qw) {
KDL::Frame pose;
int n = this->tree.getNrOfJoints();
KDL::JntArray j_q_in = JntArray(n);
for(int i = 0; i < n; i++)
j_q_in(i)=q_in[i];
this->chain_solver_fk->JntToCart(j_q_in,pose);
pose = pose * this->tool;
x = pose.p[0];
y = pose.p[1];
z = pose.p[2];
double dqx,dqy,dqz,dqw;
pose.M.GetQuaternion(dqx,dqy,dqz,dqw);
qx = dqx;
qy = dqy;
qz = dqz;
qw = dqw;
}
