1 #include "header/main_old.h" 5 int main(
int argc,
char **argv)
8 ros::init(argc, argv,
"main");
9 std::cout <<
"[girona500_A][MISSION_MANAGER] Start" << std::endl;
13 double goalLinearVect[] = {-0.287, -0.062, 7.424};
14 Eigen::Matrix4d wTgoalVeh_eigen = Eigen::Matrix4d::Identity();
16 wTgoalVeh_eigen.topLeftCorner(3,3) = Eigen::Matrix3d::Identity();
18 wTgoalVeh_eigen(0, 3) = goalLinearVect[0];
19 wTgoalVeh_eigen(1, 3) = goalLinearVect[1];
20 wTgoalVeh_eigen(2, 3) = goalLinearVect[2];
23 double goalLinearVectEE[] = {-0.27, -0.102, 2.124};
24 Eigen::Matrix4d wTgoalEE_eigen = Eigen::Matrix4d::Identity();
26 wTgoalEE_eigen.topLeftCorner(3,3) = Eigen::Matrix3d::Identity();
31 wTgoalEE_eigen(0, 3) = goalLinearVectEE[0];
32 wTgoalEE_eigen(1, 3) = goalLinearVectEE[1];
33 wTgoalEE_eigen(2, 3) = goalLinearVectEE[2];
40 std::string filename =
"/home/tori/UWsim/Peg/model/g500ARM5.urdf";
42 std::string vehicle =
"base_link";
43 std::string link0 =
"part0";
44 std::string endEffector =
"end_effector";
45 kdlHelper.setSolvers(link0, endEffector);
48 kdlHelper.getFixedFrame(vehicle, link0, &(robInfo.robotStruct.vTlink0));
52 robInfo.transforms.wTgoalVeh_eigen = wTgoalVeh_eigen;
53 robInfo.transforms.wTgoalEE_eigen = wTgoalEE_eigen;
62 RobotInterface robotInterface(
"/uwsim/g500_A/",
"girona500_A",
"girona500_B",
"pipe", argc, argv);
63 robotInterface.init();
65 robotInterface.getwTt(&(robInfo.transforms.wTt_eigen));
68 boost::asio::io_service io;
73 boost::asio::deadline_timer loopRater(io, boost::posix_time::milliseconds(ms));
75 robotInterface.getJointState(&(robInfo.robotState.jState));
76 robotInterface.getwTv(&(robInfo.robotState.
wTv_eigen));
77 robotInterface.getOtherRobPos(&(robInfo.exchangedInfo.otherRobPos));
85 kdlHelper.getEEpose(robInfo.robotState.jState, &(robInfo.robotState.link0Tee_eigen));
88 robInfo.robotState.vTee_eigen = robInfo.robotStruct.vTlink0 * robInfo.robotState.link0Tee_eigen;
91 kdlHelper.getJacobian(robInfo.robotState.jState, &(robInfo.robotState.link0_J_man));
93 computeWholeJacobian(&robInfo);
97 controller.updateTransforms(&robInfo);
99 std::vector<double> yDot = controller.execAlgorithm();
101 robotInterface.sendyDot(yDot);
103 robotInterface.spinOnce();
The Controller class is responsabile of 1) taking matrices and giving them to Tasks classes...
robotInterface: a ros node responsible of taken info from simulator and robot sensors, and of given commands back. It is the intermiate layer between actual robot and mission manager ("main")
Eigen::Matrix4d wTv_eigen
Transforms.