void RobotAlgos::Test(void)
{
//Definition of a kinematic chain & add segments to the chain
KDL::Chain chain;
chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020))));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480))));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645))));
chain.addSegment(Segment(Joint(Joint::RotZ)));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.120))));
chain.addSegment(Segment(Joint(Joint::RotZ)));
// Create solver based on kinematic chain
ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);
// Create joint array
unsigned int nj = chain.getNrOfJoints();
KDL::JntArray jointpositions = JntArray(nj);
// Assign some values to the joint positions
for(unsigned int i=0;i<nj;i++){
float myinput;
printf ("Enter the position of joint %i: ",i);
scanf ("%e",&myinput);
jointpositions(i)=(double)myinput;
}
// Create the frame that will contain the results
KDL::Frame cartpos;
// Calculate forward position kinematics
int kinematics_status;
kinematics_status = fksolver.JntToCart(jointpositions,cartpos);
if(kinematics_status>=0){
std::cout << cartpos <<std::endl;
printf("%s \n","Succes, thanks KDL!");
}else{
printf("%s \n","Error: could not calculate forward kinematics :(");
}
//Creation of the solvers:
ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver
ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
ChainIkSolverPos_NR iksolver1(chain,fksolver1,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6
//Creation of jntarrays:
JntArray result(chain.getNrOfJoints());
JntArray q_init(chain.getNrOfJoints());
//Set destination frame
Frame F_dest=cartpos;
iksolver1.CartToJnt(q_init,F_dest,result);
for(unsigned int i=0;i<nj;i++){
printf ("Axle %i: %f \n",i,result(i));
}
}
int main ( int argc, char **argv )
{
ros::init ( argc, argv, "traj_gen" );
ros::NodeHandle n;
ros::Publisher traj_pub = n.advertise<trajectory_msgs::JointTrajectory> ( "/traj_cmd",1 );
//boost::thread t2 = boost::thread::thread ( boost::bind ( &pubTrajectory ) );
ros::ServiceClient client = n.serviceClient<bosch_arm_srvs::GetJointAngles> ( "get_joint_angles" );
bosch_arm_srvs::GetJointAngles srv;
client.call ( srv );
//read destination and duration
KDL::Vector des;
for ( int i=0;i<3;i++ )
des[i]=boost::lexical_cast<double> ( argv[i+1] );
double t=boost::lexical_cast<double> ( argv[4] );
//TODO convert destination to joint angles
Segment seg0=Segment ( Joint ( Joint::None ),
Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ),Vector ( 0,0,0.27 ) ) );
Segment seg1=Segment ( Joint ( Joint::RotZ ),
Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ) ) );
Segment seg2=Segment ( Joint ( Joint::RotZ ),
Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ) ) );
Segment seg3=Segment ( Joint ( Joint::None ),
Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ),Vector ( 0,0,0.50 ) ) );
Segment seg4=Segment ( Joint ( Joint::RotZ ),
Frame ( Vector ( 0.48,0,0 ) ) );
Chain chain;
chain.addSegment ( seg0 );
chain.addSegment ( seg1 );
chain.addSegment ( seg2 );
chain.addSegment ( seg3 );
chain.addSegment ( seg4 );
ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive ( chain );
//linear interpolation in joint space
trajectory_msgs::JointTrajectory traj;
traj.header.stamp=ros::Time::now();
traj.header.frame_id="";
traj.header.seq=0;
traj.points.resize ( npts+1 );
traj.joint_names.resize ( 4 );
traj.joint_names[0]="joint1";
traj.joint_names[1]="joint2";
traj.joint_names[2]="joint3";
traj.joint_names[3]="joint4";
traj.points[i].positions.resize ( 4 );
int npts=ceil ( t*200 );
KDL::JntArray jointpositions = JntArray (3);
KDL::Frame cartpos;
double q3;
bool kinematics_status;
for ( int i=0;i<=npts;i++ )
{
//get the current joint position
client.call ( srv );
//compute forward kinematics.
jointpositions (0) =srv.response.joint_angles[0];
jointpositions (1) =srv.response.joint_angles[1];
q3 =srv.response.joint_angles[2];
jointpositions (2) =srv.response.joint_angles[3];
kinematics_status = fksolver.JntToCart ( jointpositions,cartpos );
if ( kinematics_status>=0 )
{
std::cout << cartpos.p <<std::endl;
}
else
{
printf ( "%s \n","Error: could not calculate forward kinematics :(" );
}
double steps_to_go=npts-i;
Vector step_length_cart= (des-cartpos.p)/steps_to_go;
for ( int j=0;j<4;j++ )
traj.points[i].positions[j]=srv.response.joint_angles[j]+i*dq[j];
traj.points[i].time_from_start=ros::Duration ( i*t/npts );
}
//The first message is always lost
for ( int i=0;i<2;i++ )
{
traj_pub.publish ( traj );
sleep ( 1 );
}
ros::spin();
return 0;
}
