/* * Copyright (C) 2014 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ //! [complete] #include <atomic> #include <chrono> #include <csignal> #include <iostream> #include <string> #include <thread> #include <fstream> #include <iostream> #include <string> #include <gz/msgs.hh> #include <gz/transport.hh> #include <gz_gep_tools/gz_gep_tools.hh> #include <gz_gep_tools/perception_action_loop.hh> using namespace gz_transport_hw_tools; ////////////////////////////////////////////////// int main(int argc, char **argv) { // Dealing with the arguments. int choice = 0; if (argc!=2) { std::cerr << argv[0] << " h|t" << std::endl << " h : for the H1 robot (default)" << std::endl << " t : for the TALOS robot" << std::endl; return -1; } else if (std::string(argv[1])==std::string("t")) choice = 1; // Create a transport node and advertise a topic. gz::transport::Node node; RobotCtrlJointInfos talos_ctrl_joint_infos { { "arm_left_1_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, 0.25847 , 0.0)}, { "arm_left_2_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, 0.173046, 0.0)}, { "arm_left_3_joint", ControlJointValue( 5000.0, 0.0 , 1.0, 9.0, -0.0002 , 0.0)}, { "arm_left_4_joint", ControlJointValue( 5000.0, 0.0 , 1.0, 9.0, -0.525366, 0.0)}, { "arm_left_5_joint", ControlJointValue( 500.0, 1.0 , 0.1, 5.0, 0.0, 0.0)}, { "arm_left_6_joint", ControlJointValue( 500.0, 1.0 , 0.1, 5.0, 0.0, 0.0)}, { "arm_left_7_joint", ControlJointValue( 100.0, 1.0 , 0.0, 3.0, 0.0, 0.0)}, { "arm_right_1_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, -0.25847 , 0.0)}, { "arm_right_2_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, -0.173046, 0.0)}, { "arm_right_3_joint", ControlJointValue( 5000.0, 0.0 , 1.0, 9.0, 0.0002 , 0.0)}, { "arm_right_4_joint", ControlJointValue( 5000.0, 0.0 , 1.0, 9.0, -0.525366, 0.0)}, { "arm_right_5_joint", ControlJointValue( 500.0, 1.0 , 0.1, 5.0, 0.0, 0.0)}, { "arm_right_6_joint", ControlJointValue( 500.0, 1.0 , 0.1, 5.0, 0.0, 0.0)}, { "arm_right_7_joint", ControlJointValue( 500.0, 1.0 , 0.1, 5.0, 0.0, 0.0)}, { "gripper_left_joint", ControlJointValue(1000.0,10.0 , 1.0, 10.0, 0.0, 0.0)}, { "gripper_right_joint", ControlJointValue(1000.0,10.0 , 1.0, 10.0, 0.0, 0.0)}, { "head_1_joint", ControlJointValue( 300.0, 0.1, 1.0, 5.0, 0.0, 0.0)}, { "head_2_joint", ControlJointValue( 300.0, 0.1, 1.0, 1.5, 0.0, 0.0)}, { "leg_left_1_joint", ControlJointValue( 5000.0,20.0, 5.0, 7.0, 0.0, 0.0)}, { "leg_left_2_joint", ControlJointValue( 5000.0,20.0, 5.0, 14.0, 0.0, 0.0)}, { "leg_left_3_joint", ControlJointValue( 5000.0,20.0, 5.0, 14.0,-0.448041, 0.0)}, { "leg_left_4_joint", ControlJointValue( 5000.0,20.0, 5.0, 25.0, 0.896082, 0.0)}, { "leg_left_5_joint", ControlJointValue( 5000.0,20.0, 5.0, 14.0,-0.448041, 0.0)}, { "leg_left_6_joint", ControlJointValue( 5000.0,20.0, 5.0, 9.0, 0.0, 0.0)}, { "leg_right_1_joint", ControlJointValue( 5000.0,20.0, 5.0, 7.0, 0.0, 0.0)}, { "leg_right_2_joint", ControlJointValue( 5000.0,20.0, 5.0, 14.0, 0.0, 0.0)}, { "leg_right_3_joint", ControlJointValue( 5000.0,20.0, 5.0, 14.0,-0.448041, 0.0)}, { "leg_right_4_joint", ControlJointValue( 5000.0,20.0, 5.0, 25.0, 0.896082, 0.0)}, { "leg_right_5_joint", ControlJointValue( 5000.0,20.0, 5.0, 9.0,-0.448041, 0.0)}, { "leg_right_6_joint", ControlJointValue( 5000.0,20.0, 5.0, 7.0, 0.0, 0.0)}, { "torso_1_joint", ControlJointValue( 10000.0,10.0, 1.0, 10.0, 0.0, 0.0)}, { "torso_2_joint", ControlJointValue( 10000.0,10.0, 1.0, 10.0, 0.0, 0.0)} }; gz_transport_hw_tools::RobotCtrlJointInfos h1_2_ctrl_joint_infos { { "left_shoulder_pitch_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "left_shoulder_roll_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "left_shoulder_yaw_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "left_elbow_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "left_wrist_roll_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "left_wrist_pitch_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "left_wrist_yaw_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "right_shoulder_pitch_joint",ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "right_shoulder_roll_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "right_shoulder_yaw_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "right_elbow_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "right_wrist_roll_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "right_wrist_pitch_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "right_wrist_yaw_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "left_hip_yaw_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "left_hip_pitch_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "left_hip_roll_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "left_knee_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "left_ankle_pitch_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "left_ankle_roll_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "right_hip_yaw_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "right_hip_pitch_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "right_hip_roll_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "right_knee_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "right_ankle_pitch_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "right_ankle_roll_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, { "torso_joint", ControlJointValue( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) }, }; std::string talos_prefix_model_root("/model/Pyrene/"); std::string talos_prefix_world("/world/empty_talos_gz"); std::string h1_v2_prefix_model_root("/model/H1/"); std::string h1_v2_prefix_world("/world/empty_h1_2_gz"); std::string & a_prefix_model_root = ( choice ==0 ) ? h1_v2_prefix_model_root : talos_prefix_model_root; std::string & a_prefix_world = (choice == 0) ? h1_v2_prefix_world: talos_prefix_world; gz_transport_hw_tools::RobotCtrlJointInfos & a_robot_ctrl_joint_infos = (choice==0) ? h1_2_ctrl_joint_infos: talos_ctrl_joint_infos ; bool debug_level = false; gz_transport_hw_tools::PerceptionActionLoop a_pal(a_robot_ctrl_joint_infos, a_prefix_model_root, a_prefix_world, debug_level); a_pal.InitGz(); a_pal.MainLoop(); return 0; } //! [complete]