int main(int argc, char **argv)
{
ros::init(argc, argv, "jaco_arm_driver");
ros::NodeHandle nh("~");
boost::recursive_mutex api_mutex;
bool is_first_init = true;
while (ros::ok())
{
try
{
jaco::JacoComm comm(nh, api_mutex, is_first_init);
jaco::JacoArm jaco(comm, nh);
jaco::JacoPoseActionServer pose_server(comm, nh);
jaco::JacoAnglesActionServer angles_server(comm, nh);
jaco::JacoFingersActionServer fingers_server(comm, nh);
jaco::JacoTrajectoryAnglesActionServer angle_trajectory_server(comm, nh);
jaco::JacoGripperActionServer gripper_action_server(comm, nh);
ros::spin();
}
catch(const std::exception& e)
{
ROS_ERROR_STREAM(e.what());
jaco::JacoAPI api;
boost::recursive_mutex::scoped_lock lock(api_mutex);
api.closeAPI();
ros::Duration(1.0).sleep();
}
is_first_init = false;
}
return 0;
}
Eigen::MatrixXd newtonMethodSolver(double d, double V)
{
/*Solve the non linear equation present in the matrix sys
The matrix sys contains 6 eqution represention the dimension of a box,
the aim being to get the dimension inside and outside for a defined volume V
,a thickness d of the plate and a ratio of 1x1.6x2.6
the system is solved using the Newton method iteration:
(xn+1)=(xn)-[JacobianMatrix of the System]^-1 *(system)
(yn+1) (yn)
system=[f1(xn,yn)] --> formalism for two equation two unknow
[f2(xn,yn)]
JacobianMatrix=[df1/dx df1/dy]
[df2/dx df2/dy]
More explication on the video https://www.youtube.com/watch?v=-Ws7cEH7w_U */
double a(1),b(1),c(1),A(1),B(1),C(1);// initial value of the solution (guess)
V*=0.001;// l--->m^3
//initiate the different matrix and value
Eigen::MatrixXd jaco(6,6);// jacobian matrix declaration
jaco<<JacobianInit(jaco,a,b,c);// jacobian matrix initialisation
Eigen::MatrixXd jacoInv(6,6);// jacobian matrix inverted declaration
jacoInv << jaco.inverse();// invertion of the jacobian matrix
Eigen::MatrixXd sol(6,1);// ...
sol<< A,B,C,a,b,c;// initialisation of the solution -> 1 1 1 1 1 1
Eigen::MatrixXd sys(6,1);
sys<<system2Solve(sys,sol,d,V);// initiate the system
for(int ii(0);ii<20;ii++)// number of iteration arbitrary set to 20
{
sol=sol-jacoInv*sys;
sys<<system2Solve(sys,sol,d,V);
jaco<<JacobianModif(jaco,sol);
jacoInv << jaco.inverse();
}
// cout << "Here is the matrix sol:" << endl << sol << endl;
return sol;
}
