Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <robodyn_ros/R2RosTrajectoryManager.hpp>
00003 #include <robodyn_ros/R2RosArbiter.hpp>
00004 #include <robodyn_ros/R2RosTreeId.hpp>
00005 #include <robodyn_ros/R2RosGripper.hpp>
00006
00007
00008 int main(int argc, char** argv)
00009 {
00010 ros::init(argc, argv, "R2TrajectoryManager");
00011 ros::NodeHandle nh;
00012 ros::NodeHandle pnh("~");
00013
00014 double rate;
00015 std::vector<double> priorityLevels;
00016 std::vector<double> linearTols;
00017 std::vector<double> angularTols;
00018 double momLimit;
00019 double taskGain;
00020 int ikMaxItr;
00021 double ikMaxTwist;
00022
00023 std::string controlUrdf;
00024 std::string dynamicUrdf;
00025
00026 pnh.param("rate", rate, 30.0);
00028 priorityLevels.push_back(1);
00029 priorityLevels.push_back(2);
00030 priorityLevels.push_back(3);
00031 priorityLevels.push_back(4);
00032 linearTols.push_back(0.0001);
00033 linearTols.push_back(0.001);
00034 linearTols.push_back(0.01);
00035 linearTols.push_back(0.1);
00036 angularTols.push_back(0.000174533);
00037 angularTols.push_back(0.001745329);
00038 angularTols.push_back(0.017453293);
00039 angularTols.push_back(0.174532925);
00040 momLimit = 1e-12;
00041 taskGain = 000032;
00042 ikMaxItr = 10;
00043 ikMaxTwist = 0.1;
00044 pnh.getParam("control_urdf", controlUrdf);
00045 pnh.getParam("dynamic_urdf", dynamicUrdf);
00046 R2RosTrajectoryManager trajManager(ros::this_node::getName(), controlUrdf, 1.0 / rate, pnh);
00047 trajManager.setCartesianParameters(priorityLevels, linearTols, angularTols, momLimit, taskGain, ikMaxItr, ikMaxTwist);
00048
00049 double x, y, z;
00050 std::string idMode;
00051 std::string baseFrame;
00052 nh.param("/gazebo/gravity_x", x, 0.0);
00053 nh.param("/gazebo/gravity_y", y, 0.0);
00054 nh.param("/gazebo/gravity_z", z, -9.8);
00055
00056 if (x == 0 && y == 0 && z == 0)
00057 {
00058 pnh.param("idMode", idMode, std::string("inertia"));
00059 }
00060 else
00061 {
00062 pnh.param("idMode", idMode, std::string("full"));
00063 }
00064
00065 pnh.param("baseFrame", baseFrame, std::string("r2/robot_world"));
00066 R2RosTreeId treeId(controlUrdf, pnh);
00067 treeId.setGravity(x, y, z, baseFrame, idMode);
00068
00069 R2RosArbiter arbiter(pnh);
00070
00072 R2RosGripper gripper(pnh);
00073
00074 XmlRpc::XmlRpcValue gripper_list;
00075 std::vector<std::string> gripperNames;
00076 pnh.param<XmlRpc::XmlRpcValue>("gripper/names", gripper_list, XmlRpc::XmlRpcValue());
00077
00078 if (gripper_list.valid())
00079 {
00080 ROS_ASSERT(gripper_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00081
00082 for (int32_t i = 0; i < gripper_list.size(); ++i)
00083 {
00084 ROS_ASSERT(gripper_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
00085 gripperNames.push_back(static_cast<std::string>(gripper_list[i]));
00086 }
00087 }
00088 else
00089 {
00090 gripperNames.push_back("r2/left_leg/gripper/joint0");
00091 gripperNames.push_back("r2/right_leg/gripper/joint0");
00092 }
00093
00094 double gripperVel;
00095 pnh.param<double>("gripper/velocity", gripperVel, 1.);
00096
00097
00098 double gripperAcc;
00099 pnh.param<double>("gripper/acceleration", gripperAcc, 1.);
00100
00101 gripper.configure(gripperNames, gripperVel, gripperAcc);
00102
00103 ros::Rate r(rate);
00104
00105 while (nh.ok())
00106 {
00108 trajManager.update();
00109 ros::spinOnce();
00110 r.sleep();
00111 }
00112
00113 return 0;
00114 }