├── README.md ├── assets ├── control_panel.png └── rqt_free_gait_interface.png ├── balance_controller ├── CMakeLists.txt ├── config │ ├── control.yaml │ ├── controller_gains.yaml │ ├── controller_gains_gazebo.yaml │ └── free_gait_control.perspective ├── controller_plugin_descriptions.xml ├── include │ ├── balance_controller │ │ ├── contact_force_distribution │ │ │ ├── ContactForceDistribution.hpp │ │ │ └── ContactForceDistributionBase.hpp │ │ ├── math.hpp │ │ ├── motion_control │ │ │ ├── MotionControllerBase.hpp │ │ │ └── VirtualModelController.hpp │ │ └── ros_controler │ │ │ ├── forward_command_interface.hpp │ │ │ ├── gazebo_state_hardware_interface.hpp │ │ │ ├── joint_torque_controller.hpp │ │ │ ├── joint_torque_controller.hpp.gch │ │ │ ├── robot_state_gazebo_ros_control_plugin.hpp │ │ │ ├── robot_state_gazebo_ros_control_plugin.hpp.gch │ │ │ ├── robot_state_interface.hpp │ │ │ ├── ros_balance_controller.hpp │ │ │ ├── single_leg_controller.hpp │ │ │ └── state_estimate_controller.hpp │ └── state_switcher │ │ ├── StateSwitcher.hpp │ │ └── StateSwitcherBase.hpp ├── launch │ ├── balance_controller_manager.launch │ ├── balance_controller_manager_setcap.launch │ ├── rqt_interface.launch │ └── setcap.sh ├── package.xml ├── plugin_descriptions.xml ├── src │ ├── contact_force_distribution │ │ ├── ContactForceDistribution.cpp │ │ └── ContactForceDistributionBase.cpp │ ├── motion_control │ │ ├── MotionControllerBase.cpp │ │ └── VirtualModelController.cpp │ ├── ros_controller │ │ ├── balance_controller_manager.cpp │ │ ├── gazebo_state_hardware_interface.cpp │ │ ├── joint_torque_controller.cpp │ │ ├── robot_state_gazebo_ros_control_plugin.cpp │ │ ├── ros_balance_controller.cpp │ │ ├── single_leg_controller.cpp │ │ └── state_estimate_controller.cpp │ └── state_switcher │ │ ├── StateSwitcher.cpp │ │ └── StateSwitcherBase.cpp └── test │ ├── test.cpp │ ├── test_balance_controller.cpp │ └── virtual_model_controller_test.cpp ├── control_panel ├── CMakeLists.txt ├── include │ └── rqt_control_panel_plugin │ │ ├── rqt_control_panel_plugin.h │ │ └── rqt_control_panel_plugin_widget.h ├── package.xml ├── plugin.xml ├── src │ └── rqt_control_panel_plugin │ │ ├── rqt_control_panel_plugin.cpp │ │ └── rqt_control_panel_plugin_widget.cpp └── ui │ └── rqt_control_panel_plugin │ └── rqt_control_panel_plugin_widget.ui ├── doxygen └── Doxyfile ├── free_gait_action_loader ├── CHANGELOG.rst ├── CMakeLists.txt ├── actions │ └── test.yaml ├── bin │ └── free_gait_action_loader │ │ └── action_loader.py ├── package.xml ├── setup.py └── src │ └── free_gait_action_loader │ ├── __init__.py │ ├── action_handling.py │ ├── action_handling.pyc │ ├── collection_handling.py │ └── collection_handling.pyc ├── free_gait_core ├── CHANGELOG.rst ├── CMakeLists.txt ├── doc │ ├── control_scheme.pdf │ ├── control_scheme_preview.png │ ├── motion_examples.jpg │ ├── motion_examples_preview.jpg │ ├── notions_and_coordinate_systems.pdf │ └── notions_and_coordinate_systems_preview.png ├── include │ └── free_gait_core │ │ ├── TypeDefs.hpp │ │ ├── TypePrints.hpp │ │ ├── base_motion │ │ ├── BaseAuto.hpp │ │ ├── BaseMotionBase.hpp │ │ ├── BaseTarget.hpp │ │ ├── BaseTrajectory.hpp │ │ └── base_motion.hpp │ │ ├── executor │ │ ├── AdapterBase.hpp │ │ ├── BatchExecutor.hpp │ │ ├── Executor.hpp │ │ ├── ExecutorState.hpp │ │ ├── State.hpp │ │ ├── StateBatch.hpp │ │ ├── StateBatchComputer.hpp │ │ ├── executor.hpp │ │ └── executor.hpp.gch │ │ ├── free_gait_core.hpp │ │ ├── leg_motion │ │ ├── EndEffectorMotionBase.hpp │ │ ├── EndEffectorTarget.hpp │ │ ├── EndEffectorTrajectory.hpp │ │ ├── Footstep.hpp │ │ ├── JointMotionBase.hpp │ │ ├── JointTrajectory.hpp │ │ ├── LegMode.hpp │ │ ├── LegMotionBase.hpp │ │ └── leg_motion.hpp │ │ ├── pose_optimization │ │ ├── PoseConstraintsChecker.hpp │ │ ├── PoseOptimizationBase.hpp │ │ ├── PoseOptimizationFunctionConstraints.hpp │ │ ├── PoseOptimizationGeometric.hpp │ │ ├── PoseOptimizationObjectiveFunction.hpp │ │ ├── PoseOptimizationProblem.hpp │ │ ├── PoseOptimizationQP.hpp │ │ ├── PoseOptimizationSQP.hpp │ │ ├── PoseParameterization.hpp │ │ ├── pose_optimization.hpp │ │ └── poseparameterization.h │ │ └── step │ │ ├── CustomCommand.hpp │ │ ├── Step.hpp │ │ ├── StepCompleter.hpp │ │ ├── StepComputer.hpp │ │ ├── StepParameters.hpp │ │ ├── StepQueue.hpp │ │ └── step.hpp ├── package.xml ├── plugin_description.xml ├── src │ ├── TypeDefs.cpp │ ├── TypePrints.cpp │ ├── base_motion │ │ ├── BaseAuto.cpp │ │ ├── BaseMotionBase.cpp │ │ ├── BaseTarget.cpp │ │ └── BaseTrajectory.cpp │ ├── executor │ │ ├── AdapterBase.cpp │ │ ├── BatchExecutor.cpp │ │ ├── Executor.cpp │ │ ├── ExecutorState.cpp │ │ ├── State.cpp │ │ ├── StateBatch.cpp │ │ └── StateBatchComputer.cpp │ ├── leg_motion │ │ ├── EndEffectorMotionBase.cpp │ │ ├── EndEffectorTarget.cpp │ │ ├── EndEffectorTrajectory.cpp │ │ ├── Footstep.cpp │ │ ├── JointMotionBase.cpp │ │ ├── JointTrajectory.cpp │ │ ├── LegMode.cpp │ │ └── LegMotionBase.cpp │ ├── pose_optimization │ │ ├── PoseConstraintsChecker.cpp │ │ ├── PoseOptimizationBase.cpp │ │ ├── PoseOptimizationFunctionConstraints.cpp │ │ ├── PoseOptimizationGeometric.cpp │ │ ├── PoseOptimizationObjectiveFunction.cpp │ │ ├── PoseOptimizationProblem.cpp │ │ ├── PoseOptimizationQP.cpp │ │ ├── PoseOptimizationSQP.cpp │ │ ├── PoseParameterization.cpp │ │ └── poseparameterization.cpp │ └── step │ │ ├── CustomCommand.cpp │ │ ├── Step.cpp │ │ ├── StepCompleter.cpp │ │ ├── StepComputer.cpp │ │ └── StepQueue.cpp └── test │ ├── AdapterDummy.cpp │ ├── AdapterDummy.hpp │ ├── FootstepTest.cpp │ ├── PoseOptimizationQpTest.cpp │ ├── PoseOptimizationSQPTest.cpp │ ├── StepTest.cpp │ └── test_free_gait_core.cpp ├── free_gait_marker ├── CHANGELOG.rst ├── CMakeLists.txt ├── config │ └── locomotion_controller.yaml ├── include │ └── free_gait_marker │ │ ├── marker_manager │ │ └── MarkerManager.hpp │ │ └── markers │ │ ├── MarkerFoot.hpp │ │ └── MarkerKnot.hpp ├── launch │ └── free_gait_marker.launch ├── package.xml └── src │ ├── free_gait_marker_node.cpp │ ├── marker_manager │ └── MarkerManager.cpp │ └── markers │ ├── MarkerFoot.cpp │ └── MarkerKnot.cpp ├── free_gait_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── action │ ├── ExecuteAction.action │ └── ExecuteSteps.action ├── msg │ ├── ActionDescription.msg │ ├── BaseAuto.msg │ ├── BaseTarget.msg │ ├── BaseTrajectory.msg │ ├── CollectionDescription.msg │ ├── CustomCommand.msg │ ├── EndEffectorTarget.msg │ ├── EndEffectorTrajectory.msg │ ├── Footstep.msg │ ├── JointTarget.msg │ ├── JointTrajectory.msg │ ├── LegMode.msg │ ├── RobotState.msg │ └── Step.msg ├── package.xml └── srv │ ├── GetActions.srv │ ├── GetCollections.srv │ ├── SendAction.srv │ ├── SendActionSequence.srv │ └── SetLimbConfigure.srv ├── free_gait_python ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml ├── setup.py └── src │ └── free_gait │ ├── __init__.py │ ├── action.py │ ├── action.pyc │ ├── free_gait.py │ └── free_gait.pyc ├── free_gait_ros ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── free_gait_ros │ │ ├── AdapterDummy.hpp │ │ ├── AdapterGazebo.hpp │ │ ├── AdapterGazebo.hpp.gch │ │ ├── AdapterRos.hpp │ │ ├── AdapterRosInterfaceBase.hpp │ │ ├── AdapterRosInterfaceDummy.hpp │ │ ├── AdapterRosInterfaceGazebo.hpp │ │ ├── AdapterRosInterfaceGazebo.hpp.gch │ │ ├── FootstepOptimization.hpp │ │ ├── FootstepOptimization.hpp.gch │ │ ├── FreeGaitActionClient.hpp │ │ ├── FreeGaitActionServer.hpp │ │ ├── RosVisualization.hpp │ │ ├── StateRosPublisher.hpp │ │ ├── StepFrameConverter.hpp │ │ ├── StepRosConverter.hpp │ │ ├── free_gait_ros.hpp │ │ ├── gait_generate_client.hpp │ │ └── message_traits.hpp ├── launch │ └── test.launch ├── package.xml ├── plugin_description.xml ├── src │ ├── AdapterDummy.cpp │ ├── AdapterGazebo.cpp │ ├── AdapterRos.cpp │ ├── AdapterRosInterfaceBase.cpp │ ├── AdapterRosInterfaceDummy.cpp │ ├── AdapterRosInterfaceGazebo.cpp │ ├── FreeGaitActionClient.cpp │ ├── FreeGaitActionServer.cpp │ ├── RosVisualization.cpp │ ├── StateRosPublisher.cpp │ ├── StepFrameConverter.cpp │ └── StepRosConverter.cpp └── test │ ├── AdapterDummy.cpp │ ├── AdapterDummy.hpp │ ├── FootstepOptimization.cpp │ ├── StepTest.cpp │ ├── action_client_test.cpp │ ├── action_server_test.cpp │ ├── gait_generate_client.cpp │ ├── robotStatePublisher.cpp │ ├── test.cpp │ ├── test_foothold_optimization.cpp │ └── test_free_gait_ros.cpp ├── free_gait_rviz_plugin ├── CHANGELOG.rst ├── CMakeLists.txt ├── icons │ └── classes │ │ └── FreeGaitPreview.png ├── include │ └── free_gait_rviz_plugin │ │ ├── FreeGaitPreviewDisplay.hpp │ │ ├── FreeGaitPreviewPlayback.hpp │ │ └── FreeGaitPreviewVisual.hpp ├── package.xml ├── plugin_description.xml ├── src │ ├── FreeGaitPreviewDisplay.cpp │ ├── FreeGaitPreviewPlayback.cpp │ ├── FreeGaitPreviewVisual.cpp │ └── rviz │ │ └── properties │ │ ├── button_property.cpp │ │ ├── button_property.h │ │ ├── button_toggle_property.cpp │ │ ├── button_toggle_property.h │ │ ├── float_slider_property.cpp │ │ ├── float_slider_property.h │ │ ├── line_edit_with_slider.cpp │ │ └── line_edit_with_slider.h └── test │ └── class_load_test.cpp ├── my_actions ├── CMakeLists.txt ├── actions │ └── test.yaml ├── collections │ └── test_collections.yaml ├── motion_scripts │ ├── WorkerThreadSendAction.h │ ├── base_rotate_pitch.yaml │ ├── base_rotate_roll.yaml │ ├── base_rotate_yaw.yaml │ ├── base_rotate_yaw_pitch.yaml │ ├── base_target_demo.yaml │ ├── change_to_spot.yaml │ ├── end_effectors_target_demo.yaml │ ├── leg_move.yaml │ ├── pace_gazebo.yaml │ ├── sitdown.yaml │ ├── standup.yaml │ ├── stational_walk.yaml │ └── trot_test.yaml └── package.xml ├── qp_solver ├── CMakeLists.txt ├── include │ └── qp_solver │ │ ├── QuadProg++.h │ │ ├── pose_optimization │ │ ├── PoseConstraintsChecker.hpp │ │ ├── PoseOptimizationBase.hpp │ │ ├── PoseOptimizationFunctionConstraints.hpp │ │ ├── PoseOptimizationGeometric.hpp │ │ ├── PoseOptimizationObjectiveFunction.hpp │ │ ├── PoseOptimizationProblem.hpp │ │ ├── PoseOptimizationQP.hpp │ │ ├── PoseOptimizationSQP.hpp │ │ ├── pose_optimization.hpp │ │ └── poseparameterization.h │ │ ├── qp_array.h │ │ ├── quadraticproblemsolver.h │ │ └── sequencequadraticproblemsolver.h ├── package.xml ├── src │ ├── AdapterBase.cpp │ ├── Array.cc │ ├── QuadProg++.cc │ ├── State.cpp │ ├── TypeDefs.cpp │ ├── main.cc │ ├── pose_optimization │ │ ├── PoseConstraintsChecker.cpp │ │ ├── PoseOptimizationBase.cpp │ │ ├── PoseOptimizationFunctionConstraints.cpp │ │ ├── PoseOptimizationGeometric.cpp │ │ ├── PoseOptimizationObjectiveFunction.cpp │ │ ├── PoseOptimizationProblem.cpp │ │ ├── PoseOptimizationQP.cpp │ │ ├── PoseOptimizationSQP.cpp │ │ └── poseparameterization.cpp │ ├── qp_solve_test.cpp │ ├── quadraticproblemsolver.cpp │ └── sequencequadraticproblemsolver.cpp └── test │ ├── AdapterDummy.cpp │ ├── AdapterDummy.hpp │ ├── FootstepTest.cpp │ ├── PoseOptimizationQpTest.cpp │ ├── PoseOptimizationSQPTest.cpp │ ├── StepTest.cpp │ └── test_free_gait_core.cpp ├── quadruped_model ├── CMakeLists.txt ├── include │ └── quadruped_model │ │ ├── QuadrupedModel.hpp │ │ ├── common │ │ └── typedefs.hpp │ │ ├── quadruped_state.h │ │ └── quadrupedkinematics.h ├── package.xml ├── src │ ├── QuadrupedModel.cpp │ ├── kinematicsTest.cpp │ ├── quadruped_state.cpp │ └── quadrupedkinematics.cpp └── urdf │ ├── quadruped_model.urdf │ ├── quadruped_model_lf_leg.urdf │ ├── quadruped_model_lh_leg.urdf │ ├── quadruped_model_rf_leg.urdf │ ├── quadruped_model_rh_leg.urdf │ ├── simple_humanoid.urdf │ ├── simpledog.urdf │ ├── simpledog_lf_leg.urdf │ ├── simpledog_lh_leg.urdf │ ├── simpledog_m.urdf │ ├── simpledog_rbdl.urdf │ ├── simpledog_rf_leg.urdf │ └── simpledog_rh_leg.urdf ├── rqt_free_gait_action ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── rqt_free_gait_action │ │ ├── Action.h │ │ ├── ActionModel.h │ │ ├── Collection.h │ │ ├── CollectionModel.h │ │ ├── FavoritePushButton.h │ │ ├── FreeGaitActionPlugin.h │ │ ├── WorkerThreadGetCollections.h │ │ ├── WorkerThreadSendAction.h │ │ ├── WorkerThreadSendActionSequence.h │ │ ├── WorkerThreadSwitchController.h │ │ ├── WorkerThreadUpdateTrigger.h │ │ └── workthreadsetbool.h ├── launch │ └── rqt_free_gait_action.launch ├── package.xml ├── plugin.xml ├── resource │ ├── FreeGaitActionPlugin.ui │ ├── icons │ │ ├── 16x16 │ │ │ └── refresh.svg │ │ └── 22x22 │ │ │ └── refresh.svg │ └── resources.qrc ├── scripts │ └── rqt_free_gait_action ├── setup.py └── src │ └── rqt_free_gait_action │ ├── Action.cpp │ ├── ActionModel.cpp │ ├── Collection.cpp │ ├── CollectionModel.cpp │ ├── FavoritePushButton.cpp │ ├── FreeGaitActionPlugin.cpp │ ├── WorkerThreadGetCollections.cpp │ ├── WorkerThreadSendAction.cpp │ ├── WorkerThreadSendActionSequence.cpp │ ├── WorkerThreadSwitchController.cpp │ ├── WorkerThreadUpdateTrigger.cpp │ └── workerthreadsetbool.cpp ├── rqt_free_gait_monitor ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── rqt_free_gait_monitor │ │ ├── CircularBuffer.h │ │ ├── ClickableLabel.h │ │ ├── FreeGaitMonitorPlugin.h │ │ ├── WorkerThreadPausePlay.h │ │ └── WorkerThreadStop.h ├── launch │ └── rqt_free_gait_monitor.launch ├── package.xml ├── plugin.xml ├── resource │ ├── FreeGaitMonitorPlugin.ui │ ├── icons │ │ ├── 16x16 │ │ │ ├── collapse.svg │ │ │ ├── delete.svg │ │ │ ├── done.svg │ │ │ ├── expand.svg │ │ │ ├── failed.svg │ │ │ ├── go-bottom.svg │ │ │ ├── go-down.svg │ │ │ ├── go-top.svg │ │ │ ├── go-up.svg │ │ │ ├── pause.svg │ │ │ ├── play.svg │ │ │ ├── stop.svg │ │ │ ├── unknown.svg │ │ │ └── warning.svg │ │ └── 22x22 │ │ │ ├── done.svg │ │ │ ├── failed.svg │ │ │ ├── pause.svg │ │ │ ├── play.svg │ │ │ ├── stop.svg │ │ │ ├── unknown.svg │ │ │ └── warning.svg │ └── resources.qrc ├── scripts │ └── rqt_free_gait_monitor ├── setup.py └── src │ └── rqt_free_gait_monitor │ ├── CircularBuffer.cpp │ ├── ClickableLabel.cpp │ ├── FreeGaitMonitorPlugin.cpp │ ├── WorkerThreadPausePlay.cpp │ └── WorkerThreadStop.cpp └── single_leg_test ├── CMakeLists.txt ├── CMakeLists.txt.user ├── DataFloder ├── AccelerationofForward.txt ├── Forward_Dynamics.mp4 ├── Forward_Dynamics_realsensordata.mp4 ├── PlannedData.txt ├── PositionofForward.txt ├── TauofInversedynamics.txt ├── VelocityofForward.txt ├── rf_leg_fixed_base_real_sensor.mp4 ├── rf_leg_fixed_base_whole_process.mp4 ├── webotsaccelerationreal_real.txt ├── webotsacclerationreal.txt ├── webotsposition.txt ├── webotsposition_real.txt ├── webotstorque.txt ├── webotstorquereal.txt ├── webotstorquereal_real.txt ├── webotsvelocity.txt └── webotsvelocity_real.txt ├── include └── single_leg_test │ ├── algorithm.hpp │ ├── model_test_header.hpp │ └── webotsheader_dynamics.hpp ├── lib └── model_test_header.cpp ├── package.xml ├── src └── dynamics.cpp └── test ├── example.cc ├── model_test.cpp └── test.cpp /assets/control_panel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/assets/control_panel.png -------------------------------------------------------------------------------- /assets/rqt_free_gait_interface.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/assets/rqt_free_gait_interface.png -------------------------------------------------------------------------------- /balance_controller/config/controller_gains.yaml: -------------------------------------------------------------------------------- 1 | balance_controller: 2 | virtual_model_controller: 3 | heading: 4 | kp: 5000 5 | kd: 5000 6 | kff: 10 7 | lateral: 8 | kp: 5000 9 | kd: 4000 10 | kff: 10 11 | vertical: 12 | kp: 10000 13 | kd: 5000 14 | kff: 100 15 | roll: 16 | kp: 10000 17 | kd: 1000 18 | kff: 0.2 19 | pitch: 20 | kp: 10000 21 | kd: 1000 22 | kff: 0.2 23 | yaw: 24 | kp: 4000 25 | kd: 1000 26 | kff: 1000 27 | contact_force_distribution: 28 | weights: 29 | force: 30 | heading: 1 31 | lateral: 5 32 | vertical: 1 33 | torque: 34 | roll: 10 35 | pitch: 10 36 | yaw: 5 37 | regularizer: 38 | value: 0.0001 39 | constraints: 40 | friction_coefficient: 0.6 41 | minimal_normal_force: 10 42 | single_leg_controller: 43 | x_direction: 44 | kp: 300 45 | kd: 20 46 | y_direction: 47 | kp: 300 48 | kd: 20 49 | z_direction: 50 | kp: 300 51 | kd: 20 52 | -------------------------------------------------------------------------------- /balance_controller/config/controller_gains_gazebo.yaml: -------------------------------------------------------------------------------- 1 | balance_controller: 2 | virtual_model_controller: 3 | heading: 4 | kp: 5000 5 | kd: 5000 6 | kff: 10 7 | lateral: 8 | kp: 5000 9 | kd: 4000 10 | kff: 10 11 | vertical: 12 | kp: 10000 13 | kd: 5000 14 | kff: 100 15 | roll: 16 | kp: 10000 17 | kd: 1000 18 | kff: 0.2 19 | pitch: 20 | kp: 10000 21 | kd: 1000 22 | kff: 0.2 23 | yaw: 24 | kp: 4000 25 | kd: 1000 26 | kff: 1000 27 | contact_force_distribution: 28 | weights: 29 | force: 30 | heading: 1 31 | lateral: 5 32 | vertical: 1 33 | torque: 34 | roll: 10 35 | pitch: 10 36 | yaw: 5 37 | regularizer: 38 | value: 0.0001 39 | constraints: 40 | friction_coefficient: 0.6 41 | minimal_normal_force: 10 42 | single_leg_controller: 43 | x_direction: 44 | kp: 1000 45 | kd: 200 46 | y_direction: 47 | kp: 1000 48 | kd: 200 49 | z_direction: 50 | kp: 1000 51 | kd: 200 52 | -------------------------------------------------------------------------------- /balance_controller/controller_plugin_descriptions.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | A gazebo interface with robot state . 7 | 8 | 9 | 10 | 11 | 14 | A state estimate controller. 15 | 16 | 17 | 18 | 19 | 22 | A single leg controller. 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /balance_controller/include/balance_controller/ros_controler/joint_torque_controller.hpp.gch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/balance_controller/include/balance_controller/ros_controler/joint_torque_controller.hpp.gch -------------------------------------------------------------------------------- /balance_controller/include/balance_controller/ros_controler/robot_state_gazebo_ros_control_plugin.hpp.gch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/balance_controller/include/balance_controller/ros_controler/robot_state_gazebo_ros_control_plugin.hpp.gch -------------------------------------------------------------------------------- /balance_controller/include/balance_controller/ros_controler/state_estimate_controller.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * state_estimate_controller.hpp 3 | * Descriotion: 4 | * 5 | * Created on: Jul, 7, 2019 6 | * Author: Shunyao Wang 7 | * Institute: Harbin Institute of Technology, Shenzhen 8 | */ 9 | #pragma once 10 | 11 | #include "controller_interface/controller.h" 12 | #include "balance_controller/ros_controler/robot_state_interface.hpp" 13 | #include 14 | 15 | #include "free_gait_core/free_gait_core.hpp" 16 | #include "sim_assiants/FootContacts.h" 17 | #include "sensor_msgs/Imu.h" 18 | 19 | #include "urdf/model.h" 20 | 21 | namespace balance_controller { 22 | class StateEstimateController : public controller_interface::Controller 23 | { 24 | typedef std::unordered_map LimbFlag; 25 | public: 26 | StateEstimateController(); 27 | ~StateEstimateController(); 28 | 29 | /** 30 | * @brief init 31 | * @param hardware 32 | * @param node_handle 33 | * @return 34 | */ 35 | bool init(hardware_interface::RobotStateInterface* hardware, 36 | ros::NodeHandle& node_handle); 37 | void update(const ros::Time& time, const ros::Duration& period); 38 | void starting(const ros::Time& time); 39 | void stopping(const ros::Time& time); 40 | 41 | /** 42 | * @brief joint_names 43 | */ 44 | std::vector joint_names; 45 | /** 46 | * @brief joints, a vector of joint handle, handle the joint of hardware 47 | * interface 48 | */ 49 | std::vector joints; 50 | // std::vector joints; 51 | /** 52 | * @brief robot_state_handle, handle robot state 53 | */ 54 | hardware_interface::RobotStateHandle robot_state_handle; 55 | 56 | unsigned int n_joints; 57 | 58 | private: 59 | ros::Subscriber contact_sub_, imu_sub_; 60 | 61 | LimbFlag real_contact_; 62 | std::vector limbs_; 63 | 64 | void footContactsCallback(const sim_assiants::FootContactsConstPtr& foot_contacts); 65 | void IMUmsgCallback(const sensor_msgs::ImuConstPtr& imu_msg); 66 | 67 | }; 68 | 69 | } 70 | -------------------------------------------------------------------------------- /balance_controller/include/state_switcher/StateSwitcherBase.hpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, C. Dario Bellicoso, Christian Gehring, Péter Fankhauser, Stelian Coros 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Autonomous Systems Lab nor ETH Zurich 18 | * nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | /* 36 | * StateSwitcherBase.hpp 37 | * 38 | * Created on: Oct 5, 2014 39 | * Author: C. Dario Bellicoso 40 | */ 41 | 42 | #ifndef LOCO_STATESWITCHERBASE_HPP_ 43 | #define LOCO_STATESWITCHERBASE_HPP_ 44 | 45 | 46 | namespace balance_controller { 47 | 48 | class StateSwitcherBase { 49 | public: 50 | 51 | StateSwitcherBase(); 52 | virtual ~StateSwitcherBase(); 53 | 54 | virtual bool initialize(double dt) = 0; 55 | 56 | }; 57 | 58 | } /* namespace loco */ 59 | 60 | 61 | #endif /* LOCO_STATESWITCHERBASE_HPP_ */ 62 | -------------------------------------------------------------------------------- /balance_controller/launch/balance_controller_manager_setcap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /balance_controller/launch/rqt_interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /balance_controller/launch/setcap.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Setcap does not support symbolic links, so a potential symbolic link has to be resolved first. 4 | resolved_symlink=$(readlink -f ${5}) 5 | 6 | # Setcap using password. 7 | echo ${4} | sudo -S setcap cap_net_raw+ep ${resolved_symlink} 8 | 9 | # Update the links and cache to the shared catkin libraries. 10 | # See https://stackoverflow.com/questions/9843178/linux-capabilities-setcap-seems-to-disable-ld-library-path 11 | sudo ldconfig /opt/ros/$ROS_DISTRO/lib 12 | 13 | # Launch the node. 14 | roslaunch roslaunch balance_controller balance_controller_manager.launch output:="${1}" launch_prefix:="${2}" time_step:="${3}" 15 | -------------------------------------------------------------------------------- /balance_controller/plugin_descriptions.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | A gazebo interface with robot state . 7 | 8 | 9 | 10 | 13 | A gazebo interface with robot state . 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /balance_controller/src/ros_controller/joint_torque_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * Copyright (c) 2012, hiDOF, Inc. 6 | * Copyright (c) 2013, PAL Robotics, S.L. 7 | * Copyright (c) 2014, Fraunhofer IPA 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of the Willow Garage nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | *********************************************************************/ 37 | 38 | #include 39 | #include 40 | 41 | template 42 | void forward_command_controller::ForwardJointGroupCommandController::starting(const ros::Time& time) 43 | { 44 | // Start controller with 0.0 efforts 45 | commands_buffer_.readFromRT()->assign(n_joints_, 0.0); 46 | } 47 | 48 | 49 | PLUGINLIB_EXPORT_CLASS(balance_controller::JointTorqueController, controller_interface::ControllerBase) 50 | -------------------------------------------------------------------------------- /balance_controller/src/state_switcher/StateSwitcherBase.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************************** 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, C. Dario Bellicoso, Christian Gehring, Péter Fankhauser, Stelian Coros 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Autonomous Systems Lab nor ETH Zurich 18 | * nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | /* 36 | * StateSwitcherBase.cpp 37 | * 38 | * Created on: Oct 5, 2014 39 | * Author: C. Dario Bellicoso 40 | */ 41 | 42 | #include "state_switcher/StateSwitcherBase.hpp" 43 | 44 | namespace balance_controller { 45 | 46 | 47 | StateSwitcherBase::StateSwitcherBase() { 48 | 49 | } 50 | 51 | 52 | StateSwitcherBase::~StateSwitcherBase() { 53 | 54 | } 55 | 56 | 57 | } /* namespace loco */ 58 | -------------------------------------------------------------------------------- /balance_controller/test/test_balance_controller.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * filename.cpp 3 | * Descriotion: gtest 4 | * 5 | * Created on: Mar 15, 2019 6 | * Author: Shunyao Wang 7 | * Institute: Harbin Institute of Technology, Shenzhen 8 | */ 9 | // gtest 10 | #include 11 | #include "ros/ros.h" 12 | 13 | // Run all the tests that were declared with TEST() 14 | int main(int argc, char **argv) 15 | { 16 | 17 | testing::InitGoogleTest(&argc, argv); 18 | ros::init(argc, argv, "test_balance_controller"); 19 | ros::NodeHandle nh("~"); 20 | srand((int)time(0)); 21 | return RUN_ALL_TESTS(); 22 | } 23 | -------------------------------------------------------------------------------- /balance_controller/test/virtual_model_controller_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * filename.cpp 3 | * Descriotion: 4 | * 5 | * Created on: Mar 15, 2019 6 | * Author: Shunyao Wang 7 | * Institute: Harbin Institute of Technology, Shenzhen 8 | */ 9 | 10 | // gtest 11 | #include 12 | 13 | #include "ros/ros.h" 14 | 15 | #include "free_gait_core/TypeDefs.hpp" 16 | #include "free_gait_core/free_gait_core.hpp" 17 | #include "balance_controller/contact_force_distribution/ContactForceDistributionBase.hpp" 18 | #include "balance_controller/contact_force_distribution/ContactForceDistribution.hpp" 19 | #include "balance_controller/motion_control/MotionControllerBase.hpp" 20 | #include "balance_controller/motion_control/VirtualModelController.hpp" 21 | 22 | TEST(virtual_model_controller, QPOptimization) 23 | { 24 | ros::NodeHandle nh("~"); 25 | std::shared_ptr robot_state; 26 | robot_state.reset(new free_gait::State); 27 | kindr::EulerAnglesZyxPD rotation(0.5, 0.0, 0.0); 28 | robot_state->setPoseBaseToWorld(Pose(Position(0,0,0.4),RotationQuaternion(rotation))); 29 | 30 | auto CFD = std::shared_ptr(new balance_controller::ContactForceDistribution(nh, robot_state)); 31 | ASSERT_TRUE(CFD->loadParameters()); 32 | balance_controller::VirtualModelController VMC(nh, robot_state, CFD); 33 | ASSERT_TRUE(VMC.loadParameters()); 34 | 35 | } 36 | -------------------------------------------------------------------------------- /control_panel/include/rqt_control_panel_plugin/rqt_control_panel_plugin.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROL_PANEL_PLUGIN 2 | #define CONTROL_PANEL_PLUGIN 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace rqt_control_panel_plugin { 10 | 11 | class control_panel_plugin : public rqt_gui_cpp::Plugin 12 | { 13 | public: 14 | control_panel_plugin(); 15 | 16 | void initPlugin(qt_gui_cpp::PluginContext& context) override; 17 | void shutdownPlugin() override; 18 | 19 | void saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const override; 20 | void restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings) override; 21 | 22 | private: 23 | rqt_control_panel_plugin_widget *widget = nullptr; 24 | }; 25 | 26 | } 27 | 28 | #endif // CONTROL_PANEL_PLUGIN 29 | 30 | -------------------------------------------------------------------------------- /control_panel/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | control panel for quadruped robot 4 | 5 | 6 | 7 | folder 8 | Plugins related to... 9 | 10 | 11 | control panel for quadruped robot 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /control_panel/src/rqt_control_panel_plugin/rqt_control_panel_plugin.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace rqt_control_panel_plugin { 7 | 8 | 9 | control_panel_plugin::control_panel_plugin() : 10 | rqt_gui_cpp::Plugin() 11 | { 12 | setObjectName("control_panel_plugin"); 13 | } 14 | 15 | void rqt_control_panel_plugin::control_panel_plugin::initPlugin(qt_gui_cpp::PluginContext &context) 16 | { 17 | widget = new rqt_control_panel_plugin_widget(getNodeHandle()); 18 | context.addWidget(widget); 19 | 20 | 21 | } 22 | 23 | void rqt_control_panel_plugin::control_panel_plugin::shutdownPlugin() 24 | { 25 | 26 | } 27 | 28 | void rqt_control_panel_plugin::control_panel_plugin::saveSettings(qt_gui_cpp::Settings &plugin_settings, qt_gui_cpp::Settings &instance_settings) const 29 | { 30 | 31 | } 32 | 33 | void rqt_control_panel_plugin::control_panel_plugin::restoreSettings(const qt_gui_cpp::Settings &plugin_settings, const qt_gui_cpp::Settings &instance_settings) 34 | { 35 | 36 | } 37 | 38 | } // end namespace rqt_control_panel_plugin 39 | 40 | PLUGINLIB_EXPORT_CLASS(rqt_control_panel_plugin::control_panel_plugin, rqt_gui_cpp::Plugin) 41 | -------------------------------------------------------------------------------- /free_gait_action_loader/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package free_gait_action_loader 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | 8 | 0.1.1 (2017-01-31) 9 | ------------------ 10 | * Initial release. 11 | * Contributors: Peter Fankhauser, Marko Bjelonic, Remo Diethelm 12 | -------------------------------------------------------------------------------- /free_gait_action_loader/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Project configuration 2 | cmake_minimum_required (VERSION 2.8) 3 | project(free_gait_action_loader) 4 | 5 | ## Find catkin macros and libraries 6 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 7 | ## is used, also find other catkin packages 8 | find_package(catkin REQUIRED COMPONENTS 9 | rospy 10 | free_gait_python 11 | free_gait_msgs 12 | ) 13 | 14 | catkin_package( 15 | #INCLUDE_DIRS 16 | #LIBRARIES 17 | #CATKIN_DEPENDS 18 | #DEPENDS 19 | ) 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | catkin_python_setup() 25 | 26 | ############# 27 | ## Install ## 28 | ############# 29 | 30 | # all install targets should use catkin DESTINATION variables 31 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 32 | 33 | ## Mark executable scripts (Python etc.) for installation 34 | ## in contrast to setup.py, you can choose the destination 35 | catkin_install_python(PROGRAMS 36 | bin/free_gait_action_loader/action_loader.py 37 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 38 | ) 39 | -------------------------------------------------------------------------------- /free_gait_action_loader/actions/test.yaml: -------------------------------------------------------------------------------- 1 | steps: 2 | - step: 3 | - base_auto: 4 | -------------------------------------------------------------------------------- /free_gait_action_loader/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | free_gait_action_loader 4 | 0.3.0 5 | Dynamic loading of Free Gait actions. 6 | Peter Fankhauser 7 | Peter Fankhauser 8 | BSD 9 | https://github.com/leggedrobotics/free_gait 10 | https://github.com/leggedrobotics/free_gait/issues 11 | 12 | catkin 13 | rospy 14 | free_gait_python 15 | free_gait_msgs 16 | 17 | 18 | -------------------------------------------------------------------------------- /free_gait_action_loader/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages=['free_gait_action_loader'], 6 | package_dir={'': 'src'} 7 | ) 8 | 9 | setup(**d) 10 | -------------------------------------------------------------------------------- /free_gait_action_loader/src/free_gait_action_loader/__init__.py: -------------------------------------------------------------------------------- 1 | from action_handling import ActionType 2 | from action_handling import ActionEntry 3 | from action_handling import ActionList 4 | from collection_handling import Collection 5 | from collection_handling import CollectionList 6 | -------------------------------------------------------------------------------- /free_gait_action_loader/src/free_gait_action_loader/action_handling.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/free_gait_action_loader/src/free_gait_action_loader/action_handling.pyc -------------------------------------------------------------------------------- /free_gait_action_loader/src/free_gait_action_loader/collection_handling.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/free_gait_action_loader/src/free_gait_action_loader/collection_handling.pyc -------------------------------------------------------------------------------- /free_gait_core/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package free_gait_core 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | 8 | 0.1.1 (2017-01-31) 9 | ------------------ 10 | * Initial release. 11 | * Contributors: Peter Fankhauser, Dario Bellicoso, Remo Diethelm, Thomas Bi 12 | -------------------------------------------------------------------------------- /free_gait_core/doc/control_scheme.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/free_gait_core/doc/control_scheme.pdf -------------------------------------------------------------------------------- /free_gait_core/doc/control_scheme_preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/free_gait_core/doc/control_scheme_preview.png -------------------------------------------------------------------------------- /free_gait_core/doc/motion_examples.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/free_gait_core/doc/motion_examples.jpg -------------------------------------------------------------------------------- /free_gait_core/doc/motion_examples_preview.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/free_gait_core/doc/motion_examples_preview.jpg -------------------------------------------------------------------------------- /free_gait_core/doc/notions_and_coordinate_systems.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/free_gait_core/doc/notions_and_coordinate_systems.pdf -------------------------------------------------------------------------------- /free_gait_core/doc/notions_and_coordinate_systems_preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/free_gait_core/doc/notions_and_coordinate_systems_preview.png -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/TypePrints.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TypePrints.cpp 3 | * 4 | * Created on: Nov 19, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_core/TypeDefs.hpp" 12 | 13 | namespace free_gait { 14 | 15 | std::ostream& operator<< (std::ostream& out, const ControlLevel& controlLevel); 16 | std::ostream& operator<< (std::ostream& out, const ControlSetup& controlSetup); 17 | std::ostream& operator<< (std::ostream& out, const LimbEnum& limb); 18 | std::ostream& operator<< (std::ostream& out, const BranchEnum& branch); 19 | //std::ostream& operator<< (std::ostream& out, const bool boolean); 20 | std::ostream& operator<< (std::ostream& out, const std::unordered_map& limbBooleanMap); 21 | std::ostream& operator<< (std::ostream& out, const std::unordered_map& limbVectorMap); 22 | std::ostream& operator<< (std::ostream& out, const Stance& stance); 23 | 24 | } // namespace 25 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/base_motion/base_motion.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * base_motion.hpp 3 | * 4 | * Created on: Oct 19, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_core/base_motion/BaseMotionBase.hpp" 12 | #include "free_gait_core/base_motion/BaseAuto.hpp" 13 | #include "free_gait_core/base_motion/BaseTarget.hpp" 14 | #include "free_gait_core/base_motion/BaseTrajectory.hpp" 15 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/executor/BatchExecutor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BatchExecutor.hpp 3 | * 4 | * Created on: Dec 20, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_core/executor/Executor.hpp" 12 | #include "free_gait_core/executor/AdapterBase.hpp" 13 | #include "free_gait_core/executor/StateBatch.hpp" 14 | 15 | #include //duo xian cheng 16 | #include 17 | #include //function object 18 | #include //duoxiancheng tongbu caozuo 19 | 20 | namespace free_gait { 21 | 22 | class BatchExecutor 23 | { 24 | public: 25 | BatchExecutor(free_gait::Executor& executor); 26 | virtual ~BatchExecutor(); 27 | 28 | void addProcessingCallback(std::function callback); 29 | void setTimeStep(const double timeStep); 30 | double getTimeStep() const; 31 | bool process(const std::vector& steps); 32 | bool isProcessing(); 33 | void cancelProcessing(); 34 | 35 | const StateBatch& getStateBatch() const; 36 | 37 | private: 38 | void processInThread(); 39 | 40 | StateBatch stateBatch_; 41 | free_gait::Executor& executor_; 42 | 43 | std::function callback_; 44 | double timeStep_; 45 | std::atomic isProcessing_; 46 | std::atomic requestForCancelling_; 47 | }; 48 | 49 | } /* namespace free_gait_rviz_plugin */ 50 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/executor/ExecutorState.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ExecutorState.hpp 3 | * 4 | * Created on: Mar 8, 2017 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | namespace free_gait { 14 | 15 | class ExecutorState 16 | { 17 | public: 18 | ExecutorState(); 19 | ExecutorState(size_t stepNumber, double stepPhase); 20 | virtual ~ExecutorState(); 21 | 22 | void setState(size_t stepNumber, double stepPhase); 23 | const size_t stepNumber() const; 24 | const double stepPhase() const; 25 | 26 | friend bool operator>(const ExecutorState& stateA, const ExecutorState& stateB); 27 | friend bool operator<=(const ExecutorState& stateA, const ExecutorState& stateB); 28 | friend bool operator<(const ExecutorState& stateA, const ExecutorState& stateB); 29 | friend bool operator>=(const ExecutorState& stateA, const ExecutorState& stateB); 30 | 31 | private: 32 | size_t stepNumber_; 33 | double stepPhase_; 34 | }; 35 | 36 | } /* namespace free_gait */ 37 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/executor/StateBatch.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * StateBatch.hpp 3 | * 4 | * Created on: Dec 20, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include //rongqi 16 | 17 | namespace free_gait { 18 | 19 | class StateBatch 20 | { 21 | public: 22 | StateBatch(); 23 | virtual ~StateBatch(); 24 | 25 | /** 26 | * @brief getStates 27 | * @return std::map states_; 28 | * 29 | */ 30 | const std::map& getStates() const; 31 | /** 32 | * @brief getEndEffectorPositions 33 | * @return a series of endeffectorpositions 34 | */ 35 | std::vector> getEndEffectorPositions() const; 36 | std::vector> getEndEffectorTargets() const; 37 | /** 38 | * @brief getSurfaceNormals 39 | * @return 40 | * std::tuple,means that it is a position vector, but Vector(Big V) is 41 | * typeless, they composed a tuple; 42 | * std::map>, this tuple and the double compose a map 43 | * std::vector>> 44 | */ 45 | std::vector>> getSurfaceNormals() const; 46 | std::map getStances() const; 47 | bool getEndTimeOfStep(std::string& stepId, double& endTime) const; 48 | void addState(const double time, const State& state); 49 | bool isValidTime(const double time) const; 50 | double getStartTime() const; 51 | double getEndTime() const; 52 | const State& getState(const double time) const; 53 | void clear(); 54 | 55 | friend class StateBatchComputer; 56 | 57 | private: 58 | std::map states_; 59 | std::vector> endEffectorPositions_; 60 | std::vector> endEffectorTargets_; 61 | std::vector>> surfaceNormals_; 62 | std::map stances_; 63 | std::map basePoses_; 64 | std::map stepIds_;//double is the time corresponding to the specific step 65 | }; 66 | 67 | } /* namespace free_gait */ 68 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/executor/StateBatchComputer.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * StateBatchComputer.hpp 3 | * 4 | * Created on: Dec 22, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace free_gait { 15 | 16 | class StateBatchComputer 17 | { 18 | public: 19 | StateBatchComputer(AdapterBase& adapter); 20 | virtual ~StateBatchComputer(); 21 | 22 | void computeEndEffectorTargetsAndSurfaceNormals(StateBatch& stateBatch); 23 | void computeSurfaceNormals(StateBatch& stateBatch); 24 | void computeEndEffectorTrajectories(StateBatch& stateBatch); 25 | void computeStances(StateBatch& stateBatch); 26 | void computeBaseTrajectories(StateBatch& stateBatch); 27 | void computeStepIds(StateBatch& stateBatch); 28 | 29 | private: 30 | AdapterBase& adapter_; 31 | }; 32 | 33 | } /* namespace free_gait */ 34 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/executor/executor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * executor.hpp 3 | * 4 | * Created on: Oct 22, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_core/executor/AdapterBase.hpp" 12 | #include "free_gait_core/executor/Executor.hpp" 13 | #include "free_gait_core/executor/BatchExecutor.hpp" 14 | #include "free_gait_core/executor/ExecutorState.hpp" 15 | #include "free_gait_core/executor/State.hpp" 16 | #include "free_gait_core/executor/StateBatch.hpp" 17 | #include "free_gait_core/executor/StateBatchComputer.hpp" 18 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/executor/executor.hpp.gch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/free_gait_core/include/free_gait_core/executor/executor.hpp.gch -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/free_gait_core.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * free_gait_core.hpp 3 | * 4 | * Created on: Jun 1, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_core/TypeDefs.hpp" 12 | #include "free_gait_core/executor/executor.hpp" 13 | #include "free_gait_core/step/step.hpp" 14 | #include "free_gait_core/base_motion/base_motion.hpp" 15 | #include "free_gait_core/leg_motion/leg_motion.hpp" 16 | #include "free_gait_core/pose_optimization/pose_optimization.hpp" 17 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/leg_motion/LegMode.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LegMode.hpp 3 | * 4 | * Created on: Nov 16, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | // Free Gait 12 | #include "free_gait_core/TypeDefs.hpp" 13 | #include "free_gait_core/leg_motion/EndEffectorMotionBase.hpp" 14 | 15 | namespace free_gait { 16 | 17 | class LegMode : public EndEffectorMotionBase 18 | { 19 | public: 20 | LegMode(LimbEnum limb); 21 | virtual ~LegMode(); 22 | 23 | /*! 24 | * Deep copy clone. 25 | * @return a clone of this class. 26 | */ 27 | std::unique_ptr clone() const; 28 | 29 | /*! 30 | * Update the trajectory with the foot start position. 31 | * Do this to avoid jumps of the swing leg. 32 | * @param startPosition the start position of the foot in the trajectoryFrameId_ frame. 33 | * @return true if successful, false otherwise. 34 | */ 35 | void updateStartPosition(const Position& startPosition); 36 | 37 | const ControlSetup getControlSetup() const; 38 | 39 | bool prepareComputation(const State& state, const Step& step, const AdapterBase& adapter); 40 | bool needsComputation() const; 41 | bool isComputed() const; 42 | void reset(); 43 | 44 | /*! 45 | * Evaluate the swing foot position at a given swing phase value. 46 | * @param phase the swing phase value. 47 | * @return the position of the foot on the swing trajectory. 48 | */ 49 | const Position evaluatePosition(const double time) const; 50 | 51 | /*! 52 | * Returns the total duration of the trajectory. 53 | * @return the duration. 54 | */ 55 | double getDuration() const; 56 | 57 | /*! 58 | * Return the target (end position) of the swing profile. 59 | * @return the target. 60 | */ 61 | const Position getTargetPosition() const; 62 | 63 | const std::string& getFrameId() const; 64 | 65 | bool isIgnoreContact() const; 66 | 67 | bool isIgnoreForPoseAdaptation() const; 68 | 69 | friend std::ostream& operator << (std::ostream& out, const LegMode& legMode); 70 | friend class StepCompleter; 71 | friend class StepRosConverter; 72 | 73 | private: 74 | Position position_; 75 | std::string frameId_; 76 | double duration_; 77 | bool ignoreContact_; 78 | bool ignoreForPoseAdaptation_; 79 | 80 | ControlSetup controlSetup_; 81 | 82 | //! If trajectory is updated. 83 | bool isComputed_; 84 | }; 85 | 86 | } /* namespace */ 87 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/leg_motion/leg_motion.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * leg_motion.hpp 3 | * 4 | * Created on: Oct 19, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_core/leg_motion/LegMotionBase.hpp" 12 | #include "free_gait_core/leg_motion/JointMotionBase.hpp" 13 | #include "free_gait_core/leg_motion/EndEffectorMotionBase.hpp" 14 | #include "free_gait_core/leg_motion/Footstep.hpp" 15 | #include "free_gait_core/leg_motion/EndEffectorTrajectory.hpp" 16 | #include "free_gait_core/leg_motion/EndEffectorTarget.hpp" 17 | #include "free_gait_core/leg_motion/LegMode.hpp" 18 | #include "free_gait_core/leg_motion/JointTrajectory.hpp" 19 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/pose_optimization/PoseConstraintsChecker.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PoseConstraintsChecker.hpp 3 | * 4 | * Created on: Aug 31, 2017 5 | * Author: Péter Fankhauser 6 | */ 7 | 8 | #pragma once 9 | 10 | #include "free_gait_core/pose_optimization/PoseOptimizationBase.hpp" 11 | #include "free_gait_core/TypeDefs.hpp" 12 | #include "free_gait_core/executor/AdapterBase.hpp" 13 | 14 | #include 15 | 16 | namespace free_gait { 17 | 18 | class PoseConstraintsChecker : public PoseOptimizationBase 19 | { 20 | public: 21 | PoseConstraintsChecker(const AdapterBase& adapter); 22 | virtual ~PoseConstraintsChecker(); 23 | 24 | void setTolerances(const double centerOfMassTolerance, const double legLengthTolerance); 25 | 26 | bool check(const Pose& pose); 27 | 28 | private: 29 | double centerOfMassTolerance_; 30 | double legLengthTolerance_; 31 | }; 32 | 33 | } 34 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationGeometric.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Geometric.hpp 3 | * 4 | * Created on: Aug 30, 2017 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich 7 | */ 8 | 9 | #pragma once 10 | #include "free_gait_core/pose_optimization/PoseOptimizationBase.hpp" 11 | #include "free_gait_core/TypeDefs.hpp" 12 | 13 | namespace free_gait { 14 | 15 | class PoseOptimizationGeometric : public PoseOptimizationBase 16 | { 17 | public: 18 | PoseOptimizationGeometric(const AdapterBase& adapter); 19 | virtual ~PoseOptimizationGeometric(); 20 | 21 | /*! 22 | * Set the positions of the feet of the robot in world coordinate 23 | * system for the computation of the orientation. 24 | * @param stance the position to determine the orientation. 25 | */ 26 | void setStanceForOrientation(const Stance& stance); 27 | 28 | /*! 29 | * Computes the optimized pose. 30 | * @param stanceForOrientation the positions of the feet of the robot in world coordinate 31 | * system for the computation of the orientation. 32 | * @return the optimized pose. 33 | */ 34 | bool optimize(Pose& pose); 35 | 36 | private: 37 | Stance stanceForOrientation_; 38 | }; 39 | 40 | } /* namespace loco */ 41 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationProblem.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PoseOptimizationProblem.hpp 3 | * 4 | * Created on: Mar 23, 2017 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_core/pose_optimization/PoseOptimizationObjectiveFunction.hpp" 12 | #include "free_gait_core/pose_optimization/PoseOptimizationFunctionConstraints.hpp" 13 | 14 | //#include 15 | 16 | namespace free_gait { 17 | 18 | class PoseOptimizationProblem //: public numopt_common::ConstrainedNonlinearProblem 19 | { 20 | public: 21 | PoseOptimizationProblem(std::shared_ptr objectiveFunction, 22 | std::shared_ptr functionConstraints); 23 | virtual ~PoseOptimizationProblem(); 24 | std::shared_ptr objectiveFunction_; 25 | std::shared_ptr functionConstraints_; 26 | }; 27 | 28 | } /* namespace */ 29 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationQP.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PoseOptimizationQP.hpp 3 | * 4 | * Created on: Jun 10, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_core/TypeDefs.hpp" 12 | #include "free_gait_core/pose_optimization/PoseOptimizationBase.hpp" 13 | 14 | #include 15 | #include 16 | 17 | 18 | #include 19 | #include 20 | 21 | 22 | 23 | namespace free_gait { 24 | 25 | class PoseOptimizationQP : public PoseOptimizationBase 26 | { 27 | public: 28 | PoseOptimizationQP(const AdapterBase& adapter); 29 | virtual ~PoseOptimizationQP(); 30 | // PoseOptimizationQP(const PoseOptimizationQP& other); 31 | 32 | /*! 33 | * Computes the optimized pose. 34 | * @param[in/out] pose the pose to optimize from the given initial guess. 35 | * @return true if successful, false otherwise. 36 | */ 37 | bool optimize(Pose& pose); 38 | 39 | private: 40 | std::unique_ptr solver_; 41 | unsigned int nStates_; 42 | unsigned int nDimensions_; 43 | }; 44 | 45 | } /* namespace loco */ 46 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/pose_optimization/PoseParameterization.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PoseParameterization.hpp 3 | * 4 | * Created on: Mar 22, 2017 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_core/TypeDefs.hpp" 12 | 13 | #include 14 | 15 | namespace free_gait { 16 | 17 | class PoseParameterization : public numopt_common::Parameterization 18 | { 19 | public: 20 | PoseParameterization(); 21 | virtual ~PoseParameterization(); 22 | 23 | // PoseParameterization(const PoseParameterization& other); 24 | 25 | numopt_common::Params& getParams(); 26 | const numopt_common::Params& getParams() const; 27 | 28 | bool plus(numopt_common::Params& result, const numopt_common::Params& p, 29 | const numopt_common::Delta& dp) const; 30 | 31 | bool getTransformMatrixLocalToGlobal(numopt_common::SparseMatrix& matrix, 32 | const numopt_common::Params& params) const; 33 | 34 | bool getTransformMatrixGlobalToLocal(numopt_common::SparseMatrix& matrix, 35 | const numopt_common::Params& params) const; 36 | 37 | int getGlobalSize() const; 38 | static const size_t getGlobalSizeStatic(); 39 | int getLocalSize() const; 40 | 41 | bool setRandom(numopt_common::Params& p) const; 42 | bool setIdentity(numopt_common::Params& p) const; 43 | 44 | Parameterization* clone() const; 45 | 46 | const Pose getPose() const; 47 | void setPose(const Pose& pose); 48 | const Position getPosition() const; 49 | const RotationQuaternion getOrientation() const; 50 | 51 | private: 52 | const static size_t nTransGlobal_ = 3; 53 | const static size_t nRotGlobal_ = 4; 54 | const static size_t nTransLocal_ = 3; 55 | const static size_t nRotLocal_ = 3; 56 | 57 | //! Global state vector with position and orientation. 58 | numopt_common::Params params_; 59 | }; 60 | 61 | } /* namespace free_gait */ 62 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/pose_optimization/pose_optimization.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * pose_optimization.hpp 3 | * 4 | * Created on: Oct 19, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_core/pose_optimization/PoseConstraintsChecker.hpp" 12 | #include "free_gait_core/pose_optimization/PoseOptimizationGeometric.hpp" 13 | #include "free_gait_core/pose_optimization/PoseOptimizationQP.hpp" 14 | #include "free_gait_core/pose_optimization/PoseOptimizationSQP.hpp" 15 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/pose_optimization/poseparameterization.h: -------------------------------------------------------------------------------- 1 | /* 2 | * PoseParameterization.hpp 3 | * 4 | * Created on: Mar 22, 2017 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_core/TypeDefs.hpp" 12 | 13 | #include "qp_solver/quadraticproblemsolver.h" 14 | 15 | namespace free_gait { 16 | 17 | class PoseParameterization //: public numopt_common::Parameterization 18 | { 19 | public: 20 | PoseParameterization(); 21 | virtual ~PoseParameterization(); 22 | 23 | // PoseParameterization(const PoseParameterization& other); 24 | 25 | qp_solver::QuadraticProblemSolver::parameters& getParams(); 26 | const qp_solver::QuadraticProblemSolver::parameters& getParams() const; 27 | 28 | bool plus(qp_solver::QuadraticProblemSolver::parameters& result, 29 | const qp_solver::QuadraticProblemSolver::parameters& p, 30 | const qp_solver::QuadraticProblemSolver::Delta& dp) const; 31 | 32 | bool getTransformMatrixLocalToGlobal(Eigen::MatrixXd& matrix, 33 | const qp_solver::QuadraticProblemSolver::parameters& params) const; 34 | 35 | bool getTransformMatrixGlobalToLocal(Eigen::MatrixXd& matrix, 36 | const qp_solver::QuadraticProblemSolver::parameters& params) const; 37 | 38 | int getGlobalSize() const; 39 | static const size_t getGlobalSizeStatic(); 40 | int getLocalSize() const; 41 | 42 | bool setRandom(qp_solver::QuadraticProblemSolver::parameters& p) const; 43 | bool setIdentity(qp_solver::QuadraticProblemSolver::parameters& p) const; 44 | 45 | // Parameterization* clone() const; 46 | 47 | const Pose getPose() const; 48 | void setPose(const Pose& pose); 49 | const Position getPosition() const; 50 | const RotationQuaternion getOrientation() const; 51 | 52 | private: 53 | const static size_t nTransGlobal_ = 3; 54 | const static size_t nRotGlobal_ = 4; 55 | const static size_t nTransLocal_ = 3; 56 | const static size_t nRotLocal_ = 3; 57 | 58 | //! Global state vector with position and orientation. 59 | qp_solver::QuadraticProblemSolver::parameters params_; 60 | }; 61 | 62 | } /* namespace free_gait */ 63 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/step/CustomCommand.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * CustomCommand.hpp 3 | * 4 | * Created on: Feb 15, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | // STD 12 | #include 13 | #include 14 | 15 | namespace free_gait { 16 | 17 | class CustomCommand 18 | { 19 | public: 20 | CustomCommand(); 21 | virtual ~CustomCommand(); 22 | 23 | /*! 24 | * Deep copy clone. 25 | * @return a clone of this class. 26 | */ 27 | std::unique_ptr clone() const; 28 | 29 | void setType(const std::string& type); 30 | void setDuration(const double duration); 31 | void setCommand(const std::string& command); 32 | 33 | const std::string& getType() const; 34 | double getDuration() const; 35 | const std::string& getCommand() const; 36 | 37 | friend std::ostream& operator << (std::ostream& out, const CustomCommand& customCommand); 38 | friend class StepCompleter; 39 | friend class StepRosConverter; 40 | 41 | private: 42 | std::string type_; 43 | double duration_; 44 | /** 45 | * @brief command_ 46 | * command means what?????? 47 | */ 48 | std::string command_; 49 | }; 50 | 51 | } /* namespace */ 52 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/step/StepCompleter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * StepCompleter.hpp 3 | * 4 | * Created on: Mar 7, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | // Free Gait 12 | #include "free_gait_core/TypeDefs.hpp" 13 | #include "free_gait_core/executor/State.hpp" 14 | #include "free_gait_core/executor/AdapterBase.hpp" 15 | #include "free_gait_core/step/Step.hpp" 16 | #include "free_gait_core/step/StepParameters.hpp" 17 | #include "free_gait_core/base_motion/base_motion.hpp" 18 | #include "free_gait_core/leg_motion/leg_motion.hpp" 19 | 20 | namespace free_gait { 21 | 22 | class StepCompleter 23 | { 24 | public: 25 | StepCompleter(const StepParameters& parameters, const AdapterBase& adapter); 26 | virtual ~StepCompleter(); 27 | bool complete(const State& state, const StepQueue& queue, Step& step); 28 | bool complete(const State& state, const Step& step, EndEffectorMotionBase& endEffectorMotion) const; 29 | bool complete(const State& state, const Step& step, JointMotionBase& jointMotion) const; 30 | bool complete(const State& state, const Step& step, const StepQueue& queue, BaseMotionBase& baseMotion) const; 31 | void setParameters(LegMotionBase& legMotion) const; 32 | void setParameters(Footstep& footstep) const; 33 | void setParameters(EndEffectorTarget& endEffectorMotion) const; 34 | void setParameters(LegMode& legMode) const; 35 | void setParameters(BaseAuto& baseAuto) const; 36 | void setParameters(BaseTarget& baseTarget) const; 37 | void setParameters(BaseTrajectory& baseTrajectory) const; 38 | 39 | private: 40 | const StepParameters& parameters_; 41 | const AdapterBase& adapter_; 42 | }; 43 | 44 | } /* namespace */ 45 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/step/StepComputer.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * StepComputer.hpp 3 | * 4 | * Created on: Mar 16, 2016 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_core/step/Step.hpp" 12 | 13 | namespace free_gait { 14 | 15 | class StepComputer 16 | { 17 | public: 18 | StepComputer(); 19 | virtual ~StepComputer(); 20 | virtual bool initialize(); 21 | virtual bool compute(); 22 | virtual bool isBusy(); 23 | virtual bool isDone(); 24 | virtual void resetIsDone(); 25 | void setStep(const Step& step); 26 | void getStep(Step& step); 27 | 28 | protected: 29 | Step step_; 30 | bool isDone_; 31 | }; 32 | 33 | } /* namespace */ 34 | 35 | -------------------------------------------------------------------------------- /free_gait_core/include/free_gait_core/step/step.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * step.hpp 3 | * 4 | * Created on: Oct 19, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_core/step/Step.hpp" 12 | #include "free_gait_core/step/StepQueue.hpp" 13 | #include "free_gait_core/step/StepCompleter.hpp" 14 | #include "free_gait_core/step/StepComputer.hpp" 15 | #include "free_gait_core/step/StepParameters.hpp" 16 | #include "free_gait_core/step/CustomCommand.hpp" 17 | -------------------------------------------------------------------------------- /free_gait_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | free_gait_core 4 | 0.3.0 5 | Free Gait for architecture for the control of legged robots. 6 | Peter Fankhauser 7 | Peter Fankhauser 8 | BSD 9 | https://github.com/leggedrobotics/free_gait 10 | https://github.com/leggedrobotics/free_gait/issues 11 | 12 | catkin 13 | 14 | eigen 15 | quadruped_model 16 | qp_solver 17 | curves 18 | grid_map_core 19 | robot_utils 20 | std_utils 21 | message_logger 22 | 23 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /free_gait_core/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | Display to preview Free Gait actions. 6 | 7 | 8 | -------------------------------------------------------------------------------- /free_gait_core/src/TypeDefs.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TypeDefs.cpp 3 | * 4 | * Created on: Jan 6, 2017 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #include "free_gait_core/TypeDefs.hpp" 10 | 11 | namespace free_gait { 12 | 13 | struct CompareByCounterClockwiseOrder 14 | { 15 | bool operator()(const LimbEnum& a, const LimbEnum& b) const 16 | { 17 | const size_t aIndex = findIndex(a); 18 | const size_t bIndex = findIndex(b); 19 | return aIndex < bIndex; 20 | } 21 | 22 | size_t findIndex(const LimbEnum& limb) const 23 | { 24 | auto it = std::find(limbEnumCounterClockWiseOrder.begin(), limbEnumCounterClockWiseOrder.end(), limb); 25 | if (it == limbEnumCounterClockWiseOrder.end()) { 26 | throw std::runtime_error("Could not find index for limb!"); 27 | } 28 | return std::distance(limbEnumCounterClockWiseOrder.begin(), it); 29 | } 30 | }; 31 | 32 | void getFootholdsCounterClockwiseOrdered(const Stance& stance, std::vector& footholds) 33 | { 34 | footholds.reserve(stance.size()); 35 | std::map stanceOrdered; 36 | for (const auto& limb : stance) stanceOrdered.insert(limb); 37 | for (const auto& limb : stanceOrdered) footholds.push_back(limb.second); 38 | }; 39 | 40 | } // namespace 41 | -------------------------------------------------------------------------------- /free_gait_core/src/executor/BatchExecutor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BatchExecutor.cpp 3 | * 4 | * Created on: Dec 20, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #include "free_gait_core/executor/BatchExecutor.hpp" 10 | #include "free_gait_core/executor/State.hpp" 11 | 12 | namespace free_gait { 13 | 14 | BatchExecutor::BatchExecutor(free_gait::Executor& executor) 15 | : executor_(executor), 16 | timeStep_(0.01), 17 | isProcessing_(false), 18 | requestForCancelling_(false) 19 | { 20 | } 21 | 22 | BatchExecutor::~BatchExecutor() 23 | { 24 | } 25 | 26 | void BatchExecutor::addProcessingCallback(std::function callback) 27 | { 28 | callback_ = callback; 29 | } 30 | 31 | void BatchExecutor::setTimeStep(const double timeStep) 32 | { 33 | if (isProcessing_) throw std::runtime_error("Batch executor error: Cannot change time step during processing."); 34 | timeStep_ = timeStep; 35 | } 36 | 37 | double BatchExecutor::getTimeStep() const 38 | { 39 | return timeStep_; 40 | } 41 | 42 | bool BatchExecutor::process(const std::vector& steps) 43 | { 44 | if (isProcessing_) return false; 45 | isProcessing_ = true; 46 | executor_.reset(); 47 | executor_.getQueue().add(steps); 48 | std::thread thread(std::bind(&BatchExecutor::processInThread, this)); 49 | thread.detach(); 50 | return true; 51 | } 52 | 53 | bool BatchExecutor::isProcessing() 54 | { 55 | return isProcessing_; 56 | } 57 | 58 | void BatchExecutor::cancelProcessing() 59 | { 60 | requestForCancelling_ = true; 61 | } 62 | 63 | const StateBatch& BatchExecutor::getStateBatch() const 64 | { 65 | if (isProcessing_) throw std::runtime_error("Batch executor error: Cannot access state during processing."); 66 | return stateBatch_; 67 | } 68 | 69 | void BatchExecutor::processInThread() 70 | { 71 | stateBatch_.clear(); 72 | double time = 0.0; 73 | while (!executor_.getQueue().empty() && !requestForCancelling_) { 74 | executor_.advance(timeStep_); 75 | time += timeStep_; 76 | // std::cout<<"Updated Joint Positions: "< 9 | 10 | namespace free_gait { 11 | 12 | ExecutorState::ExecutorState() 13 | : stepNumber_(0), 14 | stepPhase_(0.0) 15 | { 16 | } 17 | 18 | ExecutorState::ExecutorState(size_t stepNumber, double stepPhase) 19 | : stepNumber_(stepNumber), 20 | stepPhase_(stepPhase) 21 | { 22 | } 23 | 24 | ExecutorState::~ExecutorState() 25 | { 26 | } 27 | 28 | void ExecutorState::setState(size_t stepNumber, double stepPhase) 29 | { 30 | stepNumber_ = stepNumber; 31 | stepPhase_ = stepPhase; 32 | } 33 | 34 | const size_t ExecutorState::stepNumber() const 35 | { 36 | return stepNumber_; 37 | } 38 | 39 | const double ExecutorState::stepPhase() const 40 | { 41 | return stepPhase_; 42 | } 43 | 44 | bool operator>(const ExecutorState& stateA, const ExecutorState& stateB) 45 | { 46 | return stateA.stepNumber() >= stateB.stepNumber() && stateA.stepPhase() > stateB.stepPhase(); 47 | } 48 | 49 | bool operator>=(const ExecutorState& stateA, const ExecutorState& stateB) 50 | { 51 | return stateA.stepNumber() >= stateB.stepNumber() && stateA.stepPhase() >= stateB.stepPhase(); 52 | } 53 | 54 | bool operator<(const ExecutorState& stateA, const ExecutorState& stateB) 55 | { 56 | return stateA.stepNumber() <= stateB.stepNumber() && stateA.stepPhase() < stateB.stepPhase(); 57 | } 58 | 59 | bool operator<=(const ExecutorState& stateA, const ExecutorState& stateB) 60 | { 61 | return stateA.stepNumber() <= stateB.stepNumber() && stateA.stepPhase() <= stateB.stepPhase(); 62 | } 63 | 64 | } /* namespace free_gait */ 65 | -------------------------------------------------------------------------------- /free_gait_core/src/pose_optimization/PoseOptimizationBase.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PoseOptimizationBase.cpp 3 | * 4 | * Created on: Aug 31, 2017 5 | * Author: Péter Fankhauser 6 | */ 7 | 8 | #include "free_gait_core/pose_optimization/PoseOptimizationBase.hpp" 9 | 10 | namespace free_gait { 11 | 12 | PoseOptimizationBase::PoseOptimizationBase(const AdapterBase& adapter) 13 | : adapter_(adapter) 14 | { 15 | } 16 | 17 | PoseOptimizationBase::~PoseOptimizationBase() 18 | { 19 | } 20 | 21 | void PoseOptimizationBase::setCurrentState(const State& state) 22 | { 23 | state_ = state; 24 | } 25 | 26 | void PoseOptimizationBase::setStance(const Stance& stance) 27 | { 28 | stance_ = stance; 29 | } 30 | 31 | void PoseOptimizationBase::setSupportStance(const Stance& supportStance) 32 | { 33 | supportStance_ = supportStance; 34 | } 35 | 36 | void PoseOptimizationBase::setNominalStance(const Stance& nominalStanceInBaseFrame) 37 | { 38 | nominalStanceInBaseFrame_ = nominalStanceInBaseFrame; 39 | } 40 | 41 | void PoseOptimizationBase::setSupportRegion(const grid_map::Polygon& supportRegion) 42 | { 43 | supportRegion_ = supportRegion; 44 | } 45 | 46 | void PoseOptimizationBase::setLimbLengthConstraints(const LimbLengths& minLimbLenghts, const LimbLengths& maxLimbLenghts) 47 | { 48 | minLimbLenghts_ = minLimbLenghts; 49 | maxLimbLenghts_ = maxLimbLenghts; 50 | } 51 | 52 | void PoseOptimizationBase::checkSupportRegion() 53 | { 54 | if (supportRegion_.nVertices() == 0) { 55 | for (const auto& foot : supportStance_) 56 | supportRegion_.addVertex(foot.second.vector().head<2>()); 57 | } 58 | } 59 | /** 60 | * @brief PoseOptimizationBase::updateJointPositionsInState 61 | * @param state 62 | * @return 63 | */ 64 | bool PoseOptimizationBase::updateJointPositionsInState(State& state) const 65 | { 66 | bool totalSuccess = true; 67 | for (const auto& foot : stance_) { 68 | const Position footPositionInBase( 69 | adapter_.transformPosition(adapter_.getWorldFrameId(), adapter_.getBaseFrameId(), foot.second)); 70 | JointPositionsLeg jointPositions; 71 | const bool success = adapter_.getLimbJointPositionsFromPositionBaseToFootInBaseFrame(footPositionInBase, foot.first, jointPositions); 72 | //TODO(Shunyao): fix state feedback 73 | if (success) state.setJointPositionsForLimb(foot.first, jointPositions); 74 | else totalSuccess = totalSuccess && success; 75 | } 76 | return totalSuccess; 77 | } 78 | 79 | } 80 | -------------------------------------------------------------------------------- /free_gait_core/src/pose_optimization/PoseOptimizationProblem.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PoseOptimizationProblem.cpp 3 | * 4 | * Created on: Mar 23, 2017 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich 7 | */ 8 | #include "free_gait_core/pose_optimization/PoseOptimizationProblem.hpp" 9 | 10 | namespace free_gait { 11 | 12 | PoseOptimizationProblem::PoseOptimizationProblem( 13 | std::shared_ptr objectiveFunction, 14 | std::shared_ptr functionConstraints) 15 | //: ConstrainedNonlinearProblem(objectiveFunction, functionConstraints) 16 | : objectiveFunction_(objectiveFunction), 17 | functionConstraints_(functionConstraints) 18 | { 19 | } 20 | 21 | PoseOptimizationProblem::~PoseOptimizationProblem() 22 | { 23 | } 24 | 25 | } /* namespace */ 26 | -------------------------------------------------------------------------------- /free_gait_core/src/step/CustomCommand.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * CustomCommand.cpp 3 | * 4 | * Created on: Feb 15, 2017 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #include "free_gait_core/step/CustomCommand.hpp" 10 | 11 | // STD ss 12 | #include 13 | 14 | namespace free_gait { 15 | 16 | CustomCommand::CustomCommand() 17 | : duration_(0.0) 18 | { 19 | } 20 | 21 | CustomCommand::~CustomCommand() 22 | { 23 | } 24 | 25 | std::unique_ptr CustomCommand::clone() const 26 | { 27 | std::unique_ptr pointer(new CustomCommand(*this)); 28 | return pointer; 29 | } 30 | 31 | void CustomCommand::setType(const std::string& type) 32 | { 33 | type_ = type; 34 | } 35 | 36 | void CustomCommand::setDuration(const double duration) 37 | { 38 | duration_ = duration; 39 | } 40 | 41 | void CustomCommand::setCommand(const std::string& command) 42 | { 43 | command_ = command; 44 | } 45 | 46 | const std::string& CustomCommand::getType() const 47 | { 48 | return type_; 49 | } 50 | 51 | const std::string& CustomCommand::getCommand() const 52 | { 53 | return command_; 54 | } 55 | 56 | double CustomCommand::getDuration() const 57 | { 58 | return duration_; 59 | } 60 | 61 | std::ostream& operator<<(std::ostream& out, const CustomCommand& customCommand) 62 | { 63 | out << "Type: " << customCommand.type_ << std::endl; 64 | out << "Duration: " << customCommand.duration_ << std::endl; 65 | out << "Command: " << std::endl << customCommand.command_ << std::endl; 66 | return out; 67 | } 68 | 69 | } /* namespace */ 70 | -------------------------------------------------------------------------------- /free_gait_core/src/step/StepComputer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * StepComputer.hpp 3 | * 4 | * Created on: Mar 16, 2016 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #include "free_gait_core/step/StepComputer.hpp" 10 | 11 | namespace free_gait { 12 | /** 13 | * @brief StepComputer::StepComputer 14 | */ 15 | StepComputer::StepComputer() : 16 | isDone_(false) 17 | { 18 | } 19 | 20 | StepComputer::~StepComputer() 21 | { 22 | } 23 | 24 | bool StepComputer::initialize() 25 | { 26 | return true; 27 | } 28 | 29 | bool StepComputer::compute() 30 | { 31 | const bool result = step_.compute(); //call the method of Step::compute() 32 | isDone_ = true; 33 | return result; 34 | } 35 | 36 | bool StepComputer::isBusy() 37 | { 38 | return false; 39 | } 40 | 41 | bool StepComputer::isDone() 42 | { 43 | return isDone_; 44 | } 45 | 46 | void StepComputer::resetIsDone() 47 | { 48 | isDone_ = false; 49 | } 50 | 51 | void StepComputer::setStep(const Step& step) 52 | { 53 | step_ = step; 54 | } 55 | 56 | void StepComputer::getStep(Step& step) 57 | { 58 | step = step_; 59 | } 60 | 61 | } /* namespace */ 62 | 63 | -------------------------------------------------------------------------------- /free_gait_core/test/StepTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * StepTest.cpp 3 | * 4 | * Created on: Oct 27, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #include "free_gait_core/TypeDefs.hpp" 10 | #include "free_gait_core/step/Step.hpp" 11 | 12 | // gtest 13 | #include 14 | 15 | using namespace free_gait; 16 | 17 | TEST(step, initialization) 18 | { 19 | Step step; 20 | ASSERT_FALSE(step.hasBaseMotion()); 21 | ASSERT_FALSE(step.hasLegMotion()); 22 | ASSERT_FALSE(step.hasLegMotion(LimbEnum::RF_LEG)); 23 | Step stepCopy(step); 24 | ASSERT_FALSE(step.hasBaseMotion()); 25 | ASSERT_FALSE(step.hasLegMotion()); 26 | ASSERT_FALSE(step.hasLegMotion(LimbEnum::RF_LEG)); 27 | } 28 | -------------------------------------------------------------------------------- /free_gait_core/test/test_free_gait_core.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Created on: Jun 1, 2015 3 | * Author: Péter Fankhauser 4 | * Institute: ETH Zurich, Autonomous Systems Lab 5 | */ 6 | 7 | // gtest 8 | #include 9 | 10 | // Run all the tests that were declared with TEST() 11 | int main(int argc, char **argv) 12 | { 13 | testing::InitGoogleTest(&argc, argv); 14 | srand((int)time(0)); 15 | return RUN_ALL_TESTS(); 16 | } 17 | -------------------------------------------------------------------------------- /free_gait_marker/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package free_gait_marker 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | 8 | 0.1.1 (2017-01-31) 9 | ------------------ 10 | * Initial release. 11 | * Contributors: Peter Fankhauser, Dario Bellicoso 12 | -------------------------------------------------------------------------------- /free_gait_marker/config/locomotion_controller.yaml: -------------------------------------------------------------------------------- 1 | action_server_topic: "/free_gait/action_server" 2 | wait_for_action_timeout: 5 3 | ignore_base_motion: true 4 | foothold: 5 | frame_id: "odom" 6 | scale: 0.075 7 | radius: 0.06 8 | color: 9 | r: 0.0 10 | g: 0.0 11 | b: 0.65 12 | a: 1.0 13 | knot: 14 | frame_id: "odom" 15 | scale: 0.075 16 | radius: 0.03 17 | color: 18 | r: 1.0 19 | g: 0.0 20 | b: 0.0 21 | a: 1.0 22 | -------------------------------------------------------------------------------- /free_gait_marker/include/free_gait_marker/markers/MarkerFoot.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MarkerFoot.hpp 3 | * 4 | * Created on: Mar 18, 2015 5 | * Author: Dario Bellicoso 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace free_gait_marker { 14 | namespace markers { 15 | 16 | class MarkerFoot : public visualization_msgs::InteractiveMarker 17 | { 18 | public: 19 | MarkerFoot(); 20 | virtual ~MarkerFoot(); 21 | 22 | // Setup the marker. 23 | void setupFootholdMarker(const unsigned int stepNumber, 24 | const std::string& legName); 25 | 26 | // Load marker parameters 27 | void loadParameters( 28 | ros::NodeHandle& nodeHandle); 29 | 30 | private: 31 | //! Foothold frame id. 32 | std::string footholdFrameId_; 33 | 34 | //! Foothold marker scale. 35 | double footholdScale_; 36 | 37 | //! Foothold marker radius. 38 | double footholdRadius_; 39 | 40 | //! Foothold marker color. 41 | std_msgs::ColorRGBA footholdColor_; 42 | 43 | }; 44 | 45 | } /* namespace markers */ 46 | } /* namespace free_gait_marker */ 47 | 48 | -------------------------------------------------------------------------------- /free_gait_marker/include/free_gait_marker/markers/MarkerKnot.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MarkerKnot.hpp 3 | * 4 | * Created on: Mar 18, 2015 5 | * Author: Dario Bellicoso 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace free_gait_marker { 14 | namespace markers { 15 | 16 | class MarkerKnot : public visualization_msgs::InteractiveMarker 17 | { 18 | public: 19 | MarkerKnot(); 20 | virtual ~MarkerKnot(); 21 | 22 | // Setup the marker. 23 | void setupMarker(const unsigned int markerNumber, 24 | const std::string& markerName); 25 | 26 | // Load marker parameters 27 | void loadParameters( 28 | ros::NodeHandle& nodeHandle); 29 | 30 | private: 31 | //! Foothold marker scale. 32 | double markerScale_; 33 | 34 | //! 35 | std::string knotFrameId_; 36 | 37 | //! Foothold marker radius. 38 | double markerRadius_; 39 | 40 | //! Foothold marker color. 41 | std_msgs::ColorRGBA markerColor_; 42 | 43 | //! Timeout duration for the connection to the action server. 44 | ros::Duration waitForActionTimeout_; 45 | }; 46 | 47 | } /* namespace markers */ 48 | } /* namespace free_gait_marker */ 49 | -------------------------------------------------------------------------------- /free_gait_marker/launch/free_gait_marker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /free_gait_marker/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | free_gait_marker 4 | 0.3.0 5 | Interactive Rviz markers for 6 | Peter Fankhauser 7 | Peter Fankhauser 8 | BSD 9 | https://github.com/leggedrobotics/free_gait 10 | https://github.com/leggedrobotics/free_gait/issues 11 | 12 | catkin 13 | 14 | roscpp 15 | interactive_markers 16 | visualization_msgs 17 | geometry_msgs 18 | free_gait_msgs 19 | curves 20 | eigen 21 | multi_dof_joint_trajectory_rviz_plugins 22 | 23 | -------------------------------------------------------------------------------- /free_gait_marker/src/free_gait_marker_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * free_gait_marker_node.cpp 3 | * 4 | * Created on: Feb 18, 2015 5 | * Author: Péter Fankhauser, Dario Bellicoso 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #include 10 | 11 | #include "free_gait_marker/marker_manager/MarkerManager.hpp" 12 | 13 | int main(int argc, char** argv) 14 | { 15 | ros::init(argc, argv, "free_gait_marker"); 16 | 17 | ros::NodeHandle nodeHandle("~"); 18 | 19 | free_gait_marker::MarkerManager markerManager(nodeHandle); 20 | 21 | ros::Rate loop_rate(30); 22 | while (ros::ok()) { 23 | 24 | markerManager.publishKnots(); 25 | ros::spinOnce(); 26 | loop_rate.sleep(); 27 | } 28 | 29 | return 0; 30 | } 31 | -------------------------------------------------------------------------------- /free_gait_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package free_gait_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | 8 | 0.1.1 (2017-01-31) 9 | ------------------ 10 | * Initial release. 11 | * Contributors: Peter Fankhauser 12 | -------------------------------------------------------------------------------- /free_gait_msgs/action/ExecuteAction.action: -------------------------------------------------------------------------------- 1 | ## Goal 2 | string action_id 3 | 4 | --- 5 | 6 | ## Result 7 | int8 RESULT_FAILED = -2 8 | int8 RESULT_NOT_FOUND = -1 9 | int8 RESULT_UNKNOWN = 0 10 | int8 RESULT_STARTED = 1 11 | int8 RESULT_DONE = 2 12 | int8 status 13 | 14 | --- 15 | 16 | ## Feedback 17 | int8 STATE_ERROR = -1 18 | int8 STATE_UNINITIALIZED = 0 19 | int8 STATE_INITIALIZED = 1 20 | int8 STATE_PENDING = 2 21 | int8 STATE_ACTIVE = 3 22 | int8 STATE_IDLE = 4 23 | int8 status 24 | -------------------------------------------------------------------------------- /free_gait_msgs/action/ExecuteSteps.action: -------------------------------------------------------------------------------- 1 | ## Goal 2 | 3 | free_gait_msgs/Step[] steps 4 | 5 | # How action reacts on preemption. 6 | int8 PREEMPT_IMMEDIATE = -1 7 | int8 PREEMPT_STEP = 0 # Default. 8 | int8 PREEMPT_NO = 1 9 | int8 preempt 10 | 11 | --- 12 | 13 | ## Result 14 | # See goal status. 15 | 16 | --- 17 | 18 | ## Feedback 19 | 20 | # Unique ID of the currently active step. 21 | # If empty, step has no ID. 22 | string step_id 23 | 24 | # Step number starting from 1, monotonically increasing during 25 | # action, resets to 1 for new goal definition. Below 1 if new 26 | # goal has not started yet. 27 | uint8 step_number 28 | 29 | # Number of steps in a new/current goal. 30 | uint8 number_of_steps_in_goal 31 | 32 | # Number of steps in the queue including the current step. 33 | uint8 queue_size 34 | 35 | # Current state of the step. 36 | int8 PROGRESS_PAUSED = -1 37 | int8 PROGRESS_UNKNOWN = 0 38 | int8 PROGRESS_EXECUTING = 1 39 | int8 status 40 | 41 | # Status description ('Preparing for step.', 'Regaining contact.', etc.) 42 | # for human interpretation. 43 | string description 44 | 45 | # Duration of the current step. 46 | duration duration 47 | 48 | # Phase (0-1) of the current step. 49 | float64 phase 50 | 51 | # Names of the branches active in the current step ('LF_LEG', 'base', etc.). 52 | string[] active_branches 53 | -------------------------------------------------------------------------------- /free_gait_msgs/msg/ActionDescription.msg: -------------------------------------------------------------------------------- 1 | # Action unique id. 2 | string id 3 | 4 | # Descriptive name. 5 | string name 6 | 7 | # Absolute path to the action file. 8 | string file 9 | 10 | # Action type ('yaml', 'python', 'launch'). 11 | string type 12 | 13 | # Description of the action. 14 | string description 15 | -------------------------------------------------------------------------------- /free_gait_msgs/msg/BaseAuto.msg: -------------------------------------------------------------------------------- 1 | # Command to let controller add auto generated base motion 2 | # for stability and reachability of footsteps. 3 | 4 | # Base height in control frame. 5 | # If 0, current height of the robot is used. 6 | float64 height 7 | 8 | # If the duration of the base motion should 9 | # NOT be adapted to the duration of the step's leg. 10 | # If no leg motion is defined in the step, 11 | # the average velocities are used. 12 | # Default is false. 13 | bool ignore_timing_of_leg_motion 14 | 15 | # Average linear velocity [m/s] and angular 16 | # velocity [rad/s] of the base motion. 17 | # If 0, default is used. 18 | float64 average_linear_velocity 19 | float64 average_angular_velocity 20 | 21 | # Margin for the support polygon [m]. 22 | # If 0, default is used. 23 | float64 support_margin 24 | -------------------------------------------------------------------------------- /free_gait_msgs/msg/BaseTarget.msg: -------------------------------------------------------------------------------- 1 | # Definition of a base target pose. 2 | 3 | # Target pose of the base by the end of the motion. 4 | geometry_msgs/PoseStamped target 5 | 6 | # If the duration of the base motion should 7 | # NOT be adapted to the duration of the step's leg. 8 | # If no leg motion is defined in the step, 9 | # the average velocities are used. 10 | # Default is false. 11 | bool ignore_timing_of_leg_motion 12 | 13 | # Average linear velocity [m/s] and angular 14 | # velocity [rad/s] of the base motion. 15 | # If 0, default is used. 16 | float64 average_linear_velocity 17 | float64 average_angular_velocity 18 | -------------------------------------------------------------------------------- /free_gait_msgs/msg/BaseTrajectory.msg: -------------------------------------------------------------------------------- 1 | # Definition of a base motion trajectory. 2 | 3 | # Define the entire trajectory for the base. 4 | trajectory_msgs/MultiDOFJointTrajectory trajectory 5 | -------------------------------------------------------------------------------- /free_gait_msgs/msg/CollectionDescription.msg: -------------------------------------------------------------------------------- 1 | # Collection unique id. 2 | string id 3 | 4 | # Descriptive name. 5 | string name 6 | 7 | # List of actions IDs for this collection. 8 | string[] action_ids 9 | 10 | # Indicates if collection is a sequence of actions with a fixed order. 11 | bool is_sequence 12 | -------------------------------------------------------------------------------- /free_gait_msgs/msg/CustomCommand.msg: -------------------------------------------------------------------------------- 1 | ################### 2 | # Defines a custom command in YAML format. 3 | 4 | string type 5 | duration duration 6 | string command 7 | -------------------------------------------------------------------------------- /free_gait_msgs/msg/EndEffectorTarget.msg: -------------------------------------------------------------------------------- 1 | # Definition of a target for the end effector. 2 | 3 | # Leg name ('LF_LEG', 'RH_LEG' etc.). 4 | string name 5 | 6 | # Target of the end effector. 7 | # Target can be specified as position, velocity, acceleration, 8 | # force, or a combination of these. 9 | geometry_msgs/PointStamped[] target_position 10 | geometry_msgs/Vector3Stamped[] target_velocity 11 | geometry_msgs/Vector3Stamped[] target_acceleration 12 | geometry_msgs/Vector3Stamped[] target_force 13 | 14 | # Average velocity of the end effector motion [m/s]. 15 | # Determines the duration of the motion. 16 | # If 0, default is used. 17 | float64 average_velocity 18 | 19 | # Target surface normal. 20 | # Leave empty of no contact is expected or not known. 21 | geometry_msgs/Vector3Stamped surface_normal 22 | 23 | # If contact of the end effector should be ignored. 24 | # Default is false. 25 | bool ignore_contact 26 | 27 | # If pose adaptation should ignore this leg motion. 28 | # Default is false. 29 | bool ignore_for_pose_adaptation 30 | -------------------------------------------------------------------------------- /free_gait_msgs/msg/EndEffectorTrajectory.msg: -------------------------------------------------------------------------------- 1 | # Definition of a trajectory for the foot. 2 | 3 | # Leg name ('LF_LEG', 'RH_LEG' etc.). 4 | string name 5 | 6 | # Trajectory for the end effector. 7 | # Trajectory can contain transforms, twists, or accelerations, 8 | # or combinations of these. 9 | trajectory_msgs/MultiDOFJointTrajectory trajectory 10 | 11 | # Target surface normal. 12 | # Leave empty of no contact is expected or not known. 13 | geometry_msgs/Vector3Stamped surface_normal 14 | 15 | # If contact of the end effector should be ignored. 16 | # Default is false. 17 | bool ignore_contact 18 | 19 | # If pose adaptation should ignore this leg motion. 20 | # Default is false. 21 | bool ignore_for_pose_adaptation -------------------------------------------------------------------------------- /free_gait_msgs/msg/Footstep.msg: -------------------------------------------------------------------------------- 1 | # Step defined by foothold position and swing profile. 2 | 3 | # Leg name ('LF_LEG', 'RH_LEG' etc.). 4 | string name 5 | 6 | # Target position of the foot by the end of the motion. 7 | geometry_msgs/PointStamped target 8 | 9 | # Step apex swing heights in control frame. 10 | # If 0, default is used. 11 | float64 profile_height 12 | 13 | # Average velocity of the foot motion [m/s]. 14 | # If 0, default is used. 15 | float64 average_velocity 16 | 17 | # Type of the swing trajectory ('triangle', 'square', etc.). 18 | # If empty, default is used. 19 | string profile_type 20 | 21 | # If a contact (touchdown) of the foot at the end of the swing is not expected. 22 | # Default is false. 23 | bool ignore_contact 24 | 25 | # Target foothold surface normal. 26 | # Leave empty of no contact is expected or not known. 27 | geometry_msgs/Vector3Stamped surface_normal 28 | 29 | # If pose adaptation should ignore this leg motion. 30 | # Default is false. 31 | bool ignore_for_pose_adaptation 32 | -------------------------------------------------------------------------------- /free_gait_msgs/msg/JointTarget.msg: -------------------------------------------------------------------------------- 1 | # Definition of trajectories for each joint of a leg. 2 | 3 | # Leg name ('LF_LEG', 'RH_LEG' etc.). 4 | string name 5 | 6 | # Joint target. 7 | trajectory_msgs/JointTrajectoryPoint target 8 | 9 | # Target surface normal. 10 | # Leave empty of no contact is expected or not known. 11 | geometry_msgs/Vector3Stamped surface_normal 12 | 13 | # If contact of the end effector should be ignored. 14 | # Default is false. 15 | bool ignore_contact -------------------------------------------------------------------------------- /free_gait_msgs/msg/JointTrajectory.msg: -------------------------------------------------------------------------------- 1 | # Definition of trajectories for each joint of a leg. 2 | 3 | # Leg name ('LF_LEG', 'RH_LEG' etc.). 4 | string name 5 | 6 | # Joint trajectories. 7 | trajectory_msgs/JointTrajectory trajectory 8 | 9 | # Target surface normal. 10 | # Leave empty of no contact is expected or not known. 11 | geometry_msgs/Vector3Stamped surface_normal 12 | 13 | # If contact of the end effector should be ignored. 14 | # Default is false. 15 | bool ignore_contact -------------------------------------------------------------------------------- /free_gait_msgs/msg/LegMode.msg: -------------------------------------------------------------------------------- 1 | # Switch the leg mode between support leg and non-support leg mode. 2 | 3 | # Leg name ('LF_LEG', 'RH_LEG' etc.). 4 | string name 5 | 6 | # If leg should be a support leg or not. 7 | bool support_leg 8 | 9 | # Duration of the mode switch. 10 | # If 0, default is used. 11 | duration duration 12 | 13 | float64 phase 14 | 15 | # If support leg: Target foothold surface normal. 16 | # Leave empty if non-support leg or not known. 17 | geometry_msgs/Vector3Stamped surface_normal 18 | 19 | # If pose adaptation should ignore this leg motion. 20 | # Default is false. 21 | bool ignore_for_pose_adaptation 22 | -------------------------------------------------------------------------------- /free_gait_msgs/msg/RobotState.msg: -------------------------------------------------------------------------------- 1 | sensor_msgs/JointState lf_leg_joints 2 | sensor_msgs/JointState rf_leg_joints 3 | sensor_msgs/JointState rh_leg_joints 4 | sensor_msgs/JointState lh_leg_joints 5 | nav_msgs/Odometry base_pose 6 | free_gait_msgs/LegMode lf_leg_mode 7 | free_gait_msgs/LegMode rf_leg_mode 8 | free_gait_msgs/LegMode rh_leg_mode 9 | free_gait_msgs/LegMode lh_leg_mode 10 | free_gait_msgs/EndEffectorTarget lf_target 11 | free_gait_msgs/EndEffectorTarget rf_target 12 | free_gait_msgs/EndEffectorTarget rh_target 13 | free_gait_msgs/EndEffectorTarget lh_target 14 | -------------------------------------------------------------------------------- /free_gait_msgs/msg/Step.msg: -------------------------------------------------------------------------------- 1 | # Unique step ID (optional). 2 | string id 3 | 4 | ################### 5 | # Define leg swing motions. Each leg can be defined by one 6 | # of the following swing types: 7 | 8 | # Step defined by foothold position and swing profile. 9 | free_gait_msgs/Footstep[] footstep 10 | 11 | # Switch the leg mode between support leg and non-support leg mode. 12 | free_gait_msgs/LegMode[] leg_mode 13 | 14 | # Move end effector to a target point, with target velocity, or acceleration. 15 | free_gait_msgs/EndEffectorTarget[] end_effector_target 16 | 17 | # Move the foot along a specified trajectory. 18 | free_gait_msgs/EndEffectorTrajectory[] end_effector_trajectory 19 | 20 | # Move the foot to a target point. 21 | free_gait_msgs/JointTarget[] joint_target 22 | 23 | # Defines leg motion as trajectory for each joint. 24 | free_gait_msgs/JointTrajectory[] joint_trajectory 25 | 26 | ################### 27 | # Define the base motion by one of the base motion types: 28 | 29 | # Specify if base motion auto generation is desired or not. 30 | free_gait_msgs/BaseAuto[] base_auto 31 | 32 | # Move base to a target pose. 33 | free_gait_msgs/BaseTarget[] base_target 34 | 35 | # Move base along a trajectory. 36 | free_gait_msgs/BaseTrajectory[] base_trajectory 37 | 38 | ################### 39 | # Defines a custom command: 40 | 41 | free_gait_msgs/CustomCommand[] custom_command 42 | -------------------------------------------------------------------------------- /free_gait_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | free_gait_msgs 4 | 0.3.0 5 | ROS API definition for the Free Gait. 6 | Peter Fankhauser 7 | Peter Fankhauser 8 | BSD 9 | https://github.com/leggedrobotics/free_gait 10 | https://github.com/leggedrobotics/free_gait/issues 11 | 12 | catkin 13 | 14 | message_generation 15 | message_runtime 16 | std_msgs 17 | geometry_msgs 18 | trajectory_msgs 19 | actionlib 20 | actionlib_msgs 21 | sensor_msgs 22 | nav_msgs 23 | 24 | 25 | -------------------------------------------------------------------------------- /free_gait_msgs/srv/GetActions.srv: -------------------------------------------------------------------------------- 1 | # Get available actions for this collection, 2 | # returns all actions if `collection_id` is empty. 3 | string collection_id 4 | --- 5 | ActionDescription[] actions 6 | -------------------------------------------------------------------------------- /free_gait_msgs/srv/GetCollections.srv: -------------------------------------------------------------------------------- 1 | # Get available collections. 2 | --- 3 | CollectionDescription[] collections 4 | -------------------------------------------------------------------------------- /free_gait_msgs/srv/SendAction.srv: -------------------------------------------------------------------------------- 1 | # This sends a specified action to the step action server. 2 | # This service call is will only block until the action has been 3 | # successfully started or failed during initialization. 4 | # If you need feedback about the progress or successful 5 | # execution of the action (outside of the action itself), use 6 | # the `ExecuteAction` ROS action. 7 | 8 | free_gait_msgs/ExecuteActionGoal goal 9 | --- 10 | free_gait_msgs/ExecuteActionResult result 11 | -------------------------------------------------------------------------------- /free_gait_msgs/srv/SendActionSequence.srv: -------------------------------------------------------------------------------- 1 | # This sends a sequence of actions to the step action server. 2 | # This service call is will only block until the action has been 3 | # successfully started or failed during initialization. 4 | # If you need feedback about the progress or successful 5 | # execution of the action (outside of the action itself), use 6 | # the `ExecuteActionSequence` ROS action. 7 | 8 | free_gait_msgs/ExecuteActionGoal[] goals 9 | --- 10 | free_gait_msgs/ExecuteActionResult result 11 | -------------------------------------------------------------------------------- /free_gait_msgs/srv/SetLimbConfigure.srv: -------------------------------------------------------------------------------- 1 | string configure 2 | --- 3 | bool result 4 | -------------------------------------------------------------------------------- /free_gait_python/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package free_gait_python 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | 8 | 0.1.1 (2017-01-31) 9 | ------------------ 10 | * Initial release. 11 | * Contributors: Peter Fankhauser, Georg Wiedebach 12 | -------------------------------------------------------------------------------- /free_gait_python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Project configuration 2 | cmake_minimum_required (VERSION 2.8) 3 | project(free_gait_python) 4 | 5 | ## Find catkin macros and libraries 6 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 7 | ## is used, also find other catkin packages 8 | find_package(catkin REQUIRED COMPONENTS 9 | free_gait_msgs 10 | ) 11 | 12 | catkin_package( 13 | #INCLUDE_DIRS 14 | #LIBRARIES 15 | #CATKIN_DEPENDS 16 | #DEPENDS 17 | ) 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | catkin_python_setup() 23 | 24 | ############# 25 | ## Install ## 26 | ############# 27 | 28 | # all install targets should use catkin DESTINATION variables 29 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 30 | -------------------------------------------------------------------------------- /free_gait_python/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | free_gait_python 4 | 0.3.0 5 | A collection of python scripts for the Free Gait. 6 | Peter Fankhauser 7 | Peter Fankhauser 8 | BSD 9 | https://github.com/leggedrobotics/free_gait 10 | https://github.com/leggedrobotics/free_gait/issues 11 | 12 | catkin 13 | 14 | rospy 15 | free_gait_msgs 16 | 17 | -------------------------------------------------------------------------------- /free_gait_python/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages=['free_gait'], 6 | package_dir={'': 'src'} 7 | ) 8 | 9 | setup(**d) 10 | -------------------------------------------------------------------------------- /free_gait_python/src/free_gait/__init__.py: -------------------------------------------------------------------------------- 1 | from free_gait import get_package_path 2 | from free_gait import load_action_from_file 3 | from free_gait import parse_action 4 | from free_gait import replace_placeholders 5 | from free_gait import adapt_coordinates 6 | from free_gait import transform_coordinates 7 | from free_gait import get_transform 8 | from free_gait import transform_transformation 9 | from free_gait import transform_position 10 | from action import ActionState 11 | from action import ActionBase 12 | from action import SimpleAction 13 | from action import ContinuousAction 14 | from action import CombinedYamlAction 15 | from action import CombinedYamlActionDefinition 16 | from action import LaunchAction 17 | from action import TriggerOnFeedback 18 | from action import WaitForState 19 | -------------------------------------------------------------------------------- /free_gait_python/src/free_gait/action.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/free_gait_python/src/free_gait/action.pyc -------------------------------------------------------------------------------- /free_gait_python/src/free_gait/free_gait.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/free_gait_python/src/free_gait/free_gait.pyc -------------------------------------------------------------------------------- /free_gait_ros/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package free_gait_ros 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | 8 | 0.1.1 (2017-01-31) 9 | ------------------ 10 | * Initial release. 11 | * Contributors: Peter Fankhauser, Dario Bellicoso, Thomas Bi, Georg Wiedebach 12 | -------------------------------------------------------------------------------- /free_gait_ros/include/free_gait_ros/AdapterGazebo.hpp.gch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/free_gait_ros/include/free_gait_ros/AdapterGazebo.hpp.gch -------------------------------------------------------------------------------- /free_gait_ros/include/free_gait_ros/AdapterRos.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AdapterRos.hpp 3 | * 4 | * Created on: Dec 1, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "pluginlib_tutorials/polygon_base.h" 16 | 17 | // STD 18 | #include 19 | #include 20 | 21 | namespace free_gait { 22 | 23 | class AdapterRos 24 | { 25 | public: 26 | enum class AdapterType { 27 | Base, 28 | Preview, 29 | Gazebo 30 | }; 31 | 32 | AdapterRos(ros::NodeHandle& nodeHandle, const AdapterType type = AdapterType::Base); 33 | virtual ~AdapterRos(); 34 | bool subscribeToRobotState(const std::string& robotStateTopic = ""); 35 | void unsubscribeFromRobotState(); 36 | const std::string getRobotStateMessageType(); 37 | bool isReady() const ; 38 | bool updateAdapterWithState(); 39 | const AdapterBase& getAdapter() const; 40 | AdapterBase* getAdapterPtr(); 41 | 42 | private: 43 | ros::NodeHandle& nodeHandle_; 44 | pluginlib::ClassLoader tutor_loader_; 45 | std::unique_ptr tutor_; 46 | pluginlib::ClassLoader adapterLoader_; 47 | std::unique_ptr adapter_; 48 | pluginlib::ClassLoader adapterRosInterfaceLoader_; 49 | std::unique_ptr adapterRosInterface_; 50 | }; 51 | 52 | } /* namespace free_gait */ 53 | -------------------------------------------------------------------------------- /free_gait_ros/include/free_gait_ros/AdapterRosInterfaceBase.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AdapterRosInterfaceBase.hpp 3 | * 4 | * Created on: Nov 29, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include "free_gait_core/TypeDefs.hpp" 13 | // ROS 14 | #include 15 | #include "free_gait_msgs/RobotState.h" 16 | 17 | // STD 18 | #include 19 | 20 | namespace free_gait { 21 | 22 | class AdapterRosInterfaceBase 23 | { 24 | public: 25 | AdapterRosInterfaceBase(); 26 | virtual ~AdapterRosInterfaceBase(); 27 | void setNodeHandle(ros::NodeHandle& nodeHandle); 28 | 29 | virtual bool readRobotDescription(); 30 | virtual bool subscribeToRobotState(const std::string& robotStateTopic) = 0; 31 | virtual void unsubscribeFromRobotState() = 0; 32 | virtual const std::string getRobotStateMessageType() = 0; 33 | virtual bool isReady() const = 0; 34 | 35 | //! Update adapter. 36 | virtual bool initializeAdapter(AdapterBase& adapter) const = 0; 37 | virtual bool updateAdapterWithRobotState(AdapterBase& adapter) const = 0; 38 | 39 | // void updateRobotState(const free_gait_msgs::RobotStateConstPtr& robotState); 40 | double real_time_factor; 41 | protected: 42 | ros::NodeHandle* nodeHandle_; 43 | std::string robotDescriptionUrdfString_; 44 | 45 | // ros::Subscriber joint_states_sub_; 46 | // JointPositions all_joint_positions_; 47 | 48 | }; 49 | 50 | } /* namespace free_gait */ 51 | -------------------------------------------------------------------------------- /free_gait_ros/include/free_gait_ros/AdapterRosInterfaceDummy.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * filename.hpp 3 | * Descriotion: 4 | * 5 | * Created on: Dec 26, 2018 6 | * Author: Shunyao Wang 7 | * Institute: Harbin Institute of Technology, Shenzhen 8 | */ 9 | 10 | #pragma once 11 | #include "moveit_msgs/RobotState.h" 12 | #include "free_gait_ros/free_gait_ros.hpp" 13 | 14 | namespace free_gait { 15 | 16 | class AdapterRosInterfaceDummy : public AdapterRosInterfaceBase 17 | { 18 | public: 19 | AdapterRosInterfaceDummy(); 20 | ~AdapterRosInterfaceDummy(); 21 | 22 | virtual bool subscribeToRobotState(const std::string& robotStateTopic); 23 | virtual void unsubscribeFromRobotState(); 24 | virtual const std::string getRobotStateMessageType(); 25 | virtual bool isReady() const; 26 | 27 | //! Update adapter. 28 | virtual bool initializeAdapter(AdapterBase& adapter) const; 29 | virtual bool updateAdapterWithRobotState(AdapterBase& adapter) const; 30 | 31 | void updateRobotState(const free_gait_msgs::RobotStateConstPtr& robotState); 32 | 33 | 34 | private: 35 | ros::Subscriber joint_states_sub_; 36 | JointPositions all_joint_positions_; 37 | 38 | }; 39 | 40 | } // namespace 41 | -------------------------------------------------------------------------------- /free_gait_ros/include/free_gait_ros/AdapterRosInterfaceGazebo.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * filename.hpp 3 | * Descriotion: 4 | * 5 | * Created on: Dec 26, 2018 6 | * Author: Shunyao Wang 7 | * Institute: Harbin Institute of Technology, Shenzhen 8 | */ 9 | 10 | #pragma once 11 | #include "free_gait_msgs/RobotState.h" 12 | #include "free_gait_ros/free_gait_ros.hpp" 13 | #include "free_gait_msgs/LegMode.h" 14 | 15 | namespace free_gait { 16 | 17 | class AdapterRosInterfaceGazebo : public AdapterRosInterfaceBase 18 | { 19 | public: 20 | AdapterRosInterfaceGazebo(); 21 | ~AdapterRosInterfaceGazebo(); 22 | 23 | virtual bool subscribeToRobotState(const std::string& robotStateTopic); 24 | virtual void unsubscribeFromRobotState(); 25 | virtual const std::string getRobotStateMessageType(); 26 | virtual bool isReady() const; 27 | 28 | //! Update adapter. 29 | virtual bool initializeAdapter(AdapterBase& adapter) const; 30 | virtual bool updateAdapterWithRobotState(AdapterBase& adapter) const; 31 | 32 | void updateRobotState(const free_gait_msgs::RobotStateConstPtr& robotState); 33 | // bool isInitialized(); 34 | 35 | private: 36 | ros::Subscriber joint_states_sub_; 37 | JointPositions all_joint_positions_; 38 | JointVelocities all_joint_velocities_; 39 | nav_msgs::Odometry base_pose_in_world_; 40 | Stance foot_in_support_; 41 | std::vector leg_modes_; 42 | bool is_initialized_; 43 | 44 | }; 45 | 46 | } // namespace 47 | -------------------------------------------------------------------------------- /free_gait_ros/include/free_gait_ros/AdapterRosInterfaceGazebo.hpp.gch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/free_gait_ros/include/free_gait_ros/AdapterRosInterfaceGazebo.hpp.gch -------------------------------------------------------------------------------- /free_gait_ros/include/free_gait_ros/FootstepOptimization.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FootstepOptimization.hpp 3 | * Descriotion: 4 | * 5 | * Created on: 24, Apr, 2019 6 | * Author: Shunyao Wang 7 | * Institute: Harbin Institute of Technology, Shenzhen 8 | */ 9 | #pragma once 10 | 11 | #include "ros/ros.h" 12 | #include "grid_map_core/grid_map_core.hpp" 13 | #include "grid_map_ros/grid_map_ros.hpp" 14 | #include "grid_map_msgs/GridMap.h" 15 | #include "grid_map_ros/GridMapRosConverter.hpp" 16 | 17 | #include "free_gait_core/free_gait_core.hpp" 18 | 19 | 20 | class FootstepOptimization 21 | { 22 | public: 23 | FootstepOptimization(const ros::NodeHandle& node_handle); 24 | ~FootstepOptimization(); 25 | 26 | 27 | bool getOptimizedFoothold(free_gait::Position& nominal_foothold, 28 | const free_gait::State& robot_state, 29 | const free_gait::LimbEnum& limb, 30 | const std::string frame_id); 31 | double getMaxObstacleHeight(free_gait::Position& nominal_foothold, 32 | free_gait::State& robot_state, 33 | const free_gait::LimbEnum& limb); 34 | 35 | 36 | free_gait::Vector getSurfaceNormal(const free_gait::Position& foot_position); 37 | 38 | private: 39 | 40 | void traversabilityMapCallback(const grid_map_msgs::GridMapConstPtr& traversability_map); 41 | 42 | void initialize(); 43 | 44 | bool checkKinematicsConstriants(const free_gait::LimbEnum& limb, 45 | const grid_map::Index& index); 46 | 47 | ros::NodeHandle node_handle_; 48 | 49 | ros::Subscriber traversability_map_sub_; 50 | 51 | grid_map::GridMap traversability_map_; 52 | 53 | free_gait::State robot_state_; 54 | 55 | std::string foot_frame_id_; 56 | 57 | }; 58 | -------------------------------------------------------------------------------- /free_gait_ros/include/free_gait_ros/FootstepOptimization.hpp.gch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/free_gait_ros/include/free_gait_ros/FootstepOptimization.hpp.gch -------------------------------------------------------------------------------- /free_gait_ros/include/free_gait_ros/FreeGaitActionClient.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FreeGaitActionClient.hpp 3 | * 4 | * Created on: Oct 21, 2015 5 | * Author: Péter Fankhauser, Georg Wiedebach 6 | */ 7 | 8 | #pragma once 9 | 10 | // Free Gait 11 | #include 12 | #include 13 | 14 | // ROS 15 | #include 16 | #include 17 | 18 | // STD 19 | #include 20 | #include 21 | 22 | namespace free_gait { 23 | 24 | class FreeGaitActionClient 25 | { 26 | public: 27 | enum ActionState 28 | { 29 | ERROR, 30 | UNINITIALIZED, 31 | INITIALIZED, 32 | PENDING, 33 | ACTIVE, 34 | IDLE, 35 | DONE 36 | }; 37 | 38 | FreeGaitActionClient(ros::NodeHandle& nodeHandle); 39 | virtual ~FreeGaitActionClient() {}; 40 | 41 | void registerCallback(std::function activeCallback = nullptr, 42 | std::function feedbackCallback = nullptr, 43 | std::function doneCallback = nullptr); 45 | void sendGoal(const free_gait_msgs::ExecuteStepsGoal& goal); 46 | void sendGoal(const free_gait_msgs::ExecuteStepsGoal& goal, const bool usePreview); 47 | void waitForResult(const double timeout = 1e6); // TODO 48 | const ActionState& getState(); 49 | 50 | private: 51 | void activeCallback(); 52 | void feedbackCallback(const free_gait_msgs::ExecuteStepsFeedbackConstPtr& feedback); 53 | void doneCallback(const actionlib::SimpleClientGoalState& state, 54 | const free_gait_msgs::ExecuteStepsResultConstPtr& result); 55 | 56 | ros::NodeHandle& nodeHandle_; 57 | std::function activeCallback_; 58 | std::function feedbackCallback_; 59 | std::function doneCallback_; 61 | std::unique_ptr> client_; 62 | ros::Publisher previewPublisher_; 63 | ActionState state_; 64 | }; 65 | 66 | } 67 | -------------------------------------------------------------------------------- /free_gait_ros/include/free_gait_ros/FreeGaitActionServer.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FreeGaitActionServer.hpp 3 | * 4 | * Created on: Feb 6, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | // Free Gait 12 | #include "free_gait_core/free_gait_core.hpp" 13 | #include "free_gait_ros/StepRosConverter.hpp" 14 | 15 | // Quadruped model 16 | #include "quadruped_model/QuadrupedModel.hpp" 17 | 18 | // ROS 19 | #include 20 | #include 21 | #include 22 | 23 | // STD 24 | #include 25 | #include 26 | 27 | namespace free_gait { 28 | 29 | class FreeGaitActionServer 30 | { 31 | public: 32 | FreeGaitActionServer(ros::NodeHandle nodeHandle, const std::string& name, 33 | Executor& executor, AdapterBase& adapter); 34 | 35 | virtual ~FreeGaitActionServer(); 36 | 37 | void initialize(); 38 | // void setExecutor(std::shared_ptr executor); 39 | // void setAdapter(std::shared_ptr adapter); 40 | void start(); 41 | void update(); 42 | void shutdown(); 43 | 44 | bool isActive(); 45 | void goalCallback(); 46 | void preemptCallback(); 47 | void publishFeedback(); 48 | void setSucceeded(); 49 | void setPreempted(); 50 | void setAborted(); 51 | 52 | private: 53 | //! ROS nodehandle. 54 | ros::NodeHandle& nodeHandle_; 55 | free_gait::Executor& executor_; 56 | 57 | //! ROS converter. 58 | StepRosConverter adapter_; 59 | 60 | //! Action server. 61 | std::string name_; 62 | actionlib::SimpleActionServer server_; 63 | free_gait_msgs::ExecuteStepsActionResult result_; 64 | 65 | //! True if in process of preempting. 66 | bool isPreempting_; 67 | 68 | //! Number of steps of the current goal. 69 | size_t nStepsInCurrentGoal_; 70 | }; 71 | 72 | } /* namespace */ 73 | -------------------------------------------------------------------------------- /free_gait_ros/include/free_gait_ros/RosVisualization.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RosVisualization.hpp 3 | * 4 | * Created on: Jul 4, 2017 5 | * Author: Péter Fankhauser 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | namespace free_gait { 17 | 18 | class RosVisualization 19 | { 20 | public: 21 | RosVisualization(); 22 | virtual ~RosVisualization(); 23 | 24 | static const visualization_msgs::Marker getStanceMarker(const Stance& stance, const std::string& frameId, 25 | const std_msgs::ColorRGBA& color); 26 | 27 | static const visualization_msgs::Marker getComMarker(const Position& comPosition, const std::string& frameId, 28 | const std_msgs::ColorRGBA& color, const double size); 29 | 30 | static const visualization_msgs::MarkerArray getComWithProjectionMarker(const Position& comPosition, 31 | const std::string& frameId, 32 | const std_msgs::ColorRGBA& color, 33 | const double comMarkerSize, 34 | const double projectionLenght, 35 | const double projectionDiameter); 36 | static const visualization_msgs::MarkerArray getFootholdsMarker(const Stance& footholds, const std::string& frameId, 37 | const std_msgs::ColorRGBA& color, const double size); 38 | static const std::string getFootName(const LimbEnum& limb); 39 | }; 40 | 41 | } 42 | -------------------------------------------------------------------------------- /free_gait_ros/include/free_gait_ros/StateRosPublisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * StateRosPublisher.hpp 3 | * 4 | * Created on: Dec 6, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | // Free Gait 12 | #include 13 | #include 14 | #include 15 | 16 | // ROS 17 | #include 18 | #include 19 | 20 | // STD 21 | #include 22 | #include 23 | 24 | namespace free_gait { 25 | 26 | class StateRosPublisher 27 | { 28 | public: 29 | StateRosPublisher(ros::NodeHandle& nodeHandle, AdapterBase& adapter); 30 | virtual ~StateRosPublisher(); 31 | StateRosPublisher(const StateRosPublisher& other); 32 | 33 | void setTfPrefix(const std::string tfPrefix); 34 | bool publish(const State& state, const StepQueue& step_queue); 35 | bool publish(const State& state); 36 | private: 37 | bool initializeRobotStatePublisher(); 38 | 39 | ros::NodeHandle& nodeHandle_; 40 | std::string tfPrefix_; 41 | std::unique_ptr robotStatePublisher_; 42 | AdapterBase& adapter_; 43 | tf2_ros::TransformBroadcaster tfBroadcaster_; 44 | ros::Publisher robot_state_pub_, stance_marker_pub_; 45 | free_gait_msgs::RobotState robot_state_; 46 | }; 47 | 48 | } /* namespace free_gait */ 49 | -------------------------------------------------------------------------------- /free_gait_ros/include/free_gait_ros/free_gait_ros.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * free_gait_ros.hpp 3 | * 4 | * Created on: Jun 1, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_ros/message_traits.hpp" 12 | #include "free_gait_ros/AdapterRos.hpp" 13 | #include "free_gait_ros/AdapterRosInterfaceBase.hpp" 14 | #include "free_gait_ros/FreeGaitActionServer.hpp" 15 | #include "free_gait_ros/FreeGaitActionClient.hpp" 16 | #include "free_gait_ros/StepRosConverter.hpp" 17 | #include "free_gait_ros/StepFrameConverter.hpp" 18 | #include "free_gait_ros/StateRosPublisher.hpp" 19 | #include "free_gait_ros/RosVisualization.hpp" 20 | #include "free_gait_ros/AdapterDummy.hpp" 21 | -------------------------------------------------------------------------------- /free_gait_ros/include/free_gait_ros/message_traits.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace ros { 6 | namespace message_traits { 7 | 8 | namespace free_gait { 9 | static std::string frameId("base_link"); 10 | } 11 | 12 | template<> 13 | struct FrameId >::type > 14 | { 15 | static std::string* pointer(free_gait_msgs::ExecuteStepsActionGoal& m) 16 | { 17 | return &free_gait::frameId; 18 | } 19 | 20 | static std::string const* pointer(const free_gait_msgs::ExecuteStepsActionGoal& m) 21 | { 22 | return &free_gait::frameId; 23 | } 24 | 25 | static std::string value(const free_gait_msgs::ExecuteStepsActionGoal& m) 26 | { 27 | return free_gait::frameId; 28 | } 29 | }; 30 | 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /free_gait_ros/launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 8 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /free_gait_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | free_gait_ros 4 | 0.3.0 5 | ROS wrapper for free gait. 6 | Peter Fankhauser 7 | Peter Fankhauser 8 | BSD 9 | https://github.com/leggedrobotics/free_gait 10 | https://github.com/leggedrobotics/free_gait/issues 11 | 12 | catkin 13 | 14 | free_gait_msgs 15 | free_gait_core 16 | quadruped_model 17 | qp_solver 18 | curves_ros 19 | grid_map_core 20 | grid_map_ros 21 | pluginlib 22 | robot_state_publisher 23 | kdl_parser 24 | eigen 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /free_gait_ros/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | Display to preview Free Gait actions. 6 | 7 | 10 | Display to preview Free Gait actions. 11 | 12 | 15 | Display to preview Free Gait actions. 16 | 17 | 20 | Display to preview Free Gait actions. 21 | 22 | 23 | -------------------------------------------------------------------------------- /free_gait_ros/src/AdapterRosInterfaceBase.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AdapterRosInterfaceBase.cpp 3 | * 4 | * Created on: Nov 29, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #include 10 | 11 | #include 12 | 13 | 14 | namespace free_gait { 15 | 16 | AdapterRosInterfaceBase::AdapterRosInterfaceBase() 17 | : nodeHandle_(NULL) 18 | { 19 | } 20 | 21 | AdapterRosInterfaceBase::~AdapterRosInterfaceBase() 22 | { 23 | } 24 | 25 | void AdapterRosInterfaceBase::setNodeHandle(ros::NodeHandle& nodeHandle) 26 | { 27 | nodeHandle_ = &nodeHandle; 28 | } 29 | 30 | bool AdapterRosInterfaceBase::readRobotDescription() 31 | { 32 | std::string robotDescriptionPath; 33 | if (nodeHandle_->hasParam("/free_gait/robot_description")) { 34 | nodeHandle_->getParam("/free_gait/robot_description", robotDescriptionPath); 35 | } else { 36 | throw pluginlib::PluginlibException("Did not find ROS parameter for robot description '/free_gait/robot_description'."); 37 | } 38 | robotDescriptionUrdfString_ = robotDescriptionPath; 39 | // ROS_INFO("================================================="); 40 | // if (nodeHandle_->hasParam(robotDescriptionPath)) { 41 | // nodeHandle_->getParam(robotDescriptionPath, robotDescriptionUrdfString_); 42 | // } else { 43 | // throw pluginlib::PluginlibException("Did not find ROS parameter for robot description '" + robotDescriptionPath + "'."); 44 | // } 45 | // ROS_INFO("================================================="); 46 | if(!nodeHandle_->getParam("/real_time_factor", real_time_factor)) 47 | { 48 | ROS_ERROR("Can't find parameter of 'real_time_factor'"); 49 | return false; 50 | } 51 | 52 | return true; 53 | } 54 | bool AdapterRosInterfaceBase::updateAdapterWithRobotState(AdapterBase& adapter) const 55 | { 56 | // TODO(Shunyao): using Extra Feedback 57 | // State state = adapter.getState(); 58 | // state.setAllJointPositions(all_joint_positions_); 59 | 60 | } 61 | 62 | bool AdapterRosInterfaceBase::subscribeToRobotState(const std::string& robotStateTopic) 63 | { 64 | // topic: "/free_gait/robot_state", 65 | //joint_states_sub_ = nodeHandle_->subscribe("/robot_state", 1, &AdapterRosInterfaceBase::updateRobotState,this); 66 | 67 | } 68 | 69 | //void AdapterRosInterfaceBase::updateRobotState(const free_gait_msgs::RobotStateConstPtr& robotState) 70 | //{ 71 | // for(int i = 1;i<12;i++) 72 | // { 73 | // all_joint_positions_(i) = robotState->lf_leg_joints.position[i]; 74 | // } 75 | 76 | //} 77 | 78 | } /* namespace free_gait */ 79 | -------------------------------------------------------------------------------- /free_gait_ros/src/AdapterRosInterfaceDummy.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AdapterRosInterfaceDummy.cpp 3 | * Descriotion: Adapter Ros interface with Dummy states, use for a visulization 4 | * in Rviz 5 | * 6 | * Created on: Dec 26, 2018 7 | * Author: Shunyao Wang 8 | * Institute: Harbin Institute of Technology, Shenzhen 9 | */ 10 | 11 | #include "free_gait_ros/AdapterRosInterfaceDummy.hpp" 12 | 13 | namespace free_gait { 14 | 15 | AdapterRosInterfaceDummy::AdapterRosInterfaceDummy() 16 | { 17 | 18 | } 19 | 20 | AdapterRosInterfaceDummy::~AdapterRosInterfaceDummy() 21 | { 22 | 23 | } 24 | 25 | bool AdapterRosInterfaceDummy::subscribeToRobotState(const std::string& robotStateTopic) 26 | { 27 | joint_states_sub_ = nodeHandle_->subscribe(robotStateTopic, 1, &AdapterRosInterfaceDummy::updateRobotState,this); 28 | } 29 | void AdapterRosInterfaceDummy::unsubscribeFromRobotState() 30 | { 31 | joint_states_sub_.shutdown(); 32 | // throw std::runtime_error("AdapterRosInterfaceDummy::unsubscribeFromRobotState() is not implemented."); 33 | 34 | } 35 | const std::string AdapterRosInterfaceDummy::getRobotStateMessageType() 36 | { 37 | return ros::message_traits::datatype(); 38 | throw std::runtime_error("AdapterRosInterfaceDummy::getRobotStateMessageType() is not implemented."); 39 | 40 | } 41 | bool AdapterRosInterfaceDummy::isReady() const 42 | { 43 | throw std::runtime_error("AdapterRosInterfaceDummy::isReady() is not implemented."); 44 | 45 | } 46 | 47 | 48 | //! Update adapter. 49 | bool AdapterRosInterfaceDummy::initializeAdapter(AdapterBase& adapter) const 50 | { 51 | std::cout<<"Initial Adapter"<lf_leg_joints.position[i]; 68 | } 69 | 70 | } 71 | 72 | }//namespace 73 | 74 | 75 | #include 76 | PLUGINLIB_EXPORT_CLASS(free_gait::AdapterRosInterfaceDummy, free_gait::AdapterRosInterfaceBase) 77 | -------------------------------------------------------------------------------- /free_gait_ros/test/test_foothold_optimization.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_foothold_optimization.cpp 3 | * Descriotion: 4 | * 5 | * Created on: 24, Apr, 2019 6 | * Author: Shunyao Wang 7 | * Institute: Harbin Institute of Technology, Shenzhen 8 | */ 9 | 10 | #include "free_gait_ros/FootstepOptimization.hpp" 11 | 12 | int main(int argc, char *argv[]) 13 | { 14 | ros::init(argc, argv, "foothold_optimization_test_node"); 15 | ros::NodeHandle nh("~"); 16 | FootstepOptimization footstepOptimiztion(nh); 17 | // footstepOptimiztion.initialize(); 18 | ros::Rate rate(100); 19 | free_gait::State robot_state; 20 | robot_state.setPoseBaseToWorld(Pose(Position(4,-3,0.4), RotationQuaternion(1,0,0,0))); 21 | 22 | Position foothold_in_world(4.5, -3.175, 0); 23 | while (ros::ok()) { 24 | footstepOptimiztion.getOptimizedFoothold(foothold_in_world, robot_state, free_gait::LimbEnum::LF_LEG,"odom"); 25 | ROS_INFO_STREAM("Optimized Foot hold position "< 9 | 10 | // Run all the tests that were declared with TEST() 11 | int main(int argc, char **argv) 12 | { 13 | testing::InitGoogleTest(&argc, argv); 14 | srand((int)time(0)); 15 | return RUN_ALL_TESTS(); 16 | } 17 | -------------------------------------------------------------------------------- /free_gait_rviz_plugin/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package free_gait_rviz_plugin 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | 8 | 0.1.1 (2017-01-31) 9 | ------------------ 10 | * Initial release. 11 | * Contributors: Peter Fankhauser 12 | -------------------------------------------------------------------------------- /free_gait_rviz_plugin/icons/classes/FreeGaitPreview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/free_gait_rviz_plugin/icons/classes/FreeGaitPreview.png -------------------------------------------------------------------------------- /free_gait_rviz_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | free_gait_rviz_plugin 4 | 0.3.0 5 | RViz plugin to preview Free Gait actions. 6 | Peter Fankhauser 7 | Peter Fankhauser 8 | BSD 9 | https://github.com/leggedrobotics/free_gait 10 | https://github.com/leggedrobotics/free_gait/issues 11 | 12 | catkin 13 | 14 | qtbase5-dev 15 | rviz 16 | free_gait_core 17 | free_gait_ros 18 | free_gait_msgs 19 | 20 | libqt5-core 21 | libqt5-gui 22 | libqt5-widgets 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /free_gait_rviz_plugin/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | Display to preview Free Gait actions. 6 | free_gait_msgs/ExecuteStepsActionGoal 7 | 8 | 9 | -------------------------------------------------------------------------------- /free_gait_rviz_plugin/src/rviz/properties/button_property.cpp: -------------------------------------------------------------------------------- 1 | #include "rviz/properties/button_property.h" 2 | 3 | namespace rviz 4 | { 5 | 6 | ButtonProperty::ButtonProperty(const QString& name, const QString& default_value, 7 | const QString& description, Property* parent, 8 | const char *changed_slot, QObject* receiver) 9 | : Property(name, default_value, description, parent, changed_slot, receiver), 10 | label_(default_value) 11 | { 12 | setShouldBeSaved(false); 13 | } 14 | 15 | void ButtonProperty::setLabel(const std::string& label) 16 | { 17 | label_ = QString::fromStdString(label); 18 | } 19 | 20 | QVariant ButtonProperty::getViewData( int column, int role ) const 21 | { 22 | if (column == 1) { 23 | switch (role) { 24 | case Qt::DisplayRole: 25 | return label_; 26 | case Qt::EditRole: 27 | case Qt::CheckStateRole: 28 | return QVariant(); 29 | default: 30 | return Property::getViewData(column, role); 31 | } 32 | } 33 | return Property::getViewData(column, role); 34 | } 35 | 36 | Qt::ItemFlags ButtonProperty::getViewFlags( int column ) const 37 | { 38 | Qt::ItemFlags enabled_flag = ( getParent() && getParent()->getDisableChildren() ) ? Qt::NoItemFlags : Qt::ItemIsEnabled; 39 | if (column == 0) return Property::getViewFlags(column); 40 | return Qt::ItemIsEditable | enabled_flag | Qt::ItemIsSelectable; 41 | } 42 | 43 | QWidget* ButtonProperty::createEditor(QWidget* parent, const QStyleOptionViewItem& option) 44 | { 45 | QPushButton* button = new QPushButton(label_, parent); 46 | connect(button, SIGNAL(released()), this, SLOT(buttonReleased())); 47 | return button; 48 | } 49 | 50 | void ButtonProperty::buttonReleased() 51 | { 52 | Q_EMIT changed(); 53 | } 54 | 55 | } // end namespace rviz 56 | -------------------------------------------------------------------------------- /free_gait_rviz_plugin/src/rviz/properties/button_property.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "rviz/properties/property.h" 6 | #include 7 | 8 | namespace rviz 9 | { 10 | 11 | class ButtonProperty: public Property 12 | { 13 | Q_OBJECT 14 | public: 15 | ButtonProperty(const QString& name = QString(), const QString& default_value = QString(), 16 | const QString& description = QString(), Property* parent = 0, 17 | const char* changed_slot = 0, QObject* receiver = 0); 18 | 19 | void setLabel(const std::string& label); 20 | QVariant getViewData( int column, int role ) const; 21 | Qt::ItemFlags getViewFlags( int column ) const; 22 | 23 | public Q_SLOTS: 24 | void buttonReleased(); 25 | 26 | protected: 27 | virtual QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option); 28 | 29 | private: 30 | QString label_; 31 | }; 32 | 33 | } // end namespace rviz 34 | 35 | -------------------------------------------------------------------------------- /free_gait_rviz_plugin/src/rviz/properties/button_toggle_property.cpp: -------------------------------------------------------------------------------- 1 | #include "rviz/properties/button_toggle_property.h" 2 | 3 | namespace rviz 4 | { 5 | 6 | ButtonToggleProperty::ButtonToggleProperty(const QString& name, bool default_value, 7 | const QString& description, Property* parent, 8 | const char *changed_slot, QObject* receiver) 9 | : ButtonProperty(name, "", description, parent, changed_slot, receiver), 10 | labelForTrue_("True"), 11 | labelForFalse_("False"), 12 | button_(NULL) 13 | { 14 | } 15 | 16 | void ButtonToggleProperty::setLabels(const std::string& labelForTrue, const std::string& labelForFalse) 17 | { 18 | labelForTrue_ = QString::fromStdString(labelForTrue); 19 | labelForFalse_ = QString::fromStdString(labelForFalse); 20 | } 21 | 22 | const QString& ButtonToggleProperty::getLabel() 23 | { 24 | return (getBool() ? labelForTrue_ : labelForFalse_); 25 | } 26 | 27 | bool ButtonToggleProperty::getBool() const 28 | { 29 | return getValue().toBool(); 30 | } 31 | 32 | QWidget* ButtonToggleProperty::createEditor(QWidget* parent, const QStyleOptionViewItem& option) 33 | { 34 | button_ = new QPushButton(getLabel(), parent); 35 | button_->setCheckable(true); 36 | button_->setChecked(true); 37 | connect(button_, SIGNAL(toggled(bool)), this, SLOT(buttonToggled(bool))); 38 | return button_; 39 | } 40 | 41 | void ButtonToggleProperty::buttonToggled(bool state) 42 | { 43 | button_->setText(getLabel()); 44 | value_.setValue(state); 45 | Q_EMIT changed(); 46 | } 47 | 48 | } // end namespace rviz 49 | -------------------------------------------------------------------------------- /free_gait_rviz_plugin/src/rviz/properties/button_toggle_property.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "rviz/properties/button_property.h" 4 | 5 | namespace rviz 6 | { 7 | 8 | class ButtonToggleProperty: public ButtonProperty 9 | { 10 | Q_OBJECT 11 | public: 12 | ButtonToggleProperty(const QString& name = QString(), bool default_value = false, 13 | const QString& description = QString(), Property* parent = 0, 14 | const char* changed_slot = 0, QObject* receiver = 0); 15 | 16 | void setLabels(const std::string& labelForTrue, const std::string& labelForFalse); 17 | const QString& getLabel(); 18 | bool getBool() const; 19 | 20 | public Q_SLOTS: 21 | void buttonToggled(bool state); 22 | 23 | protected: 24 | virtual QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option); 25 | 26 | QString labelForTrue_; 27 | QString labelForFalse_; 28 | QPushButton* button_; 29 | }; 30 | 31 | } // end namespace rviz 32 | 33 | -------------------------------------------------------------------------------- /free_gait_rviz_plugin/src/rviz/properties/float_slider_property.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * float_slider_property.cpp 3 | * 4 | * Created on: Dec 12, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #include "rviz/properties/float_slider_property.h" 10 | #include "rviz/properties/line_edit_with_slider.h" 11 | 12 | #include 13 | #include 14 | 15 | namespace rviz 16 | { 17 | 18 | FloatSliderProperty::FloatSliderProperty(const QString& name, float default_value, 19 | const QString& description, Property* parent, 20 | const char *changed_slot, QObject* receiver) 21 | : FloatProperty(name, default_value, description, parent, changed_slot, receiver), 22 | minIntValue_(0), 23 | maxIntValue_(1000) 24 | { 25 | setMin(0.0); 26 | setMax(1.0); 27 | } 28 | 29 | bool FloatSliderProperty::setValuePassive(const QVariant& value) 30 | { 31 | value_ = qBound(getMin(), value.toFloat(), getMax()); 32 | return true; 33 | } 34 | 35 | void FloatSliderProperty::sliderValueChanged(int value) 36 | { 37 | const float floatValue = getMin() + (getMax() - getMin()) 38 | * (value - minIntValue_) / (maxIntValue_ - minIntValue_); 39 | setValue(floatValue); 40 | } 41 | 42 | QWidget* FloatSliderProperty::createEditor(QWidget* parent, const QStyleOptionViewItem& option) 43 | { 44 | LineEditWithSlider* slider = new LineEditWithSlider(parent); 45 | slider->slider()->setRange(minIntValue_, maxIntValue_); 46 | slider->setValidator(new QDoubleValidator(slider->slider())); 47 | const int intValue = minIntValue_ + (maxIntValue_ - minIntValue_) 48 | * (getValue().toFloat() - getMin()) / (getMax() - getMin()); 49 | slider->slider()->setValue(intValue); 50 | connect(slider->slider(), SIGNAL(valueChanged(int)), this, SLOT(sliderValueChanged(int))); 51 | return slider; 52 | } 53 | 54 | } // end namespace rviz 55 | -------------------------------------------------------------------------------- /free_gait_rviz_plugin/src/rviz/properties/float_slider_property.h: -------------------------------------------------------------------------------- 1 | /* 2 | * float_slider_property.h 3 | * 4 | * Created on: Dec 12, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace rviz { 16 | 17 | class FloatSliderProperty : public FloatProperty 18 | { 19 | Q_OBJECT 20 | public: 21 | FloatSliderProperty(const QString& name = QString(), float default_value = 0.0, 22 | const QString& description = QString(), Property* parent = 0, 23 | const char* changed_slot = 0, QObject* receiver = 0); 24 | 25 | virtual bool setValuePassive(const QVariant& value); 26 | 27 | public Q_SLOTS: 28 | virtual void sliderValueChanged(int value); 29 | 30 | protected: 31 | virtual QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option); 32 | 33 | private: 34 | int minIntValue_, maxIntValue_; 35 | }; 36 | 37 | } // end namespace rviz 38 | 39 | -------------------------------------------------------------------------------- /free_gait_rviz_plugin/src/rviz/properties/line_edit_with_slider.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * line_edit_with_slider.cpp 3 | * 4 | * Created on: Dec 12, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #include "rviz/properties/line_edit_with_slider.h" 10 | 11 | #include 12 | 13 | namespace rviz 14 | { 15 | 16 | LineEditWithSlider::LineEditWithSlider( QWidget* parent ) 17 | : QLineEdit( parent ) 18 | { 19 | setFrame(false); 20 | slider_ = new QSlider(Qt::Orientation::Horizontal, this); 21 | } 22 | 23 | void LineEditWithSlider::resizeEvent( QResizeEvent* event ) 24 | { 25 | int padding = 0; 26 | int textMinimalWidth = 45; 27 | int sliderWidth = width() - textMinimalWidth - 2; 28 | setTextMargins(padding, 0, sliderWidth, 0); 29 | QLineEdit::resizeEvent(event); 30 | slider_->setGeometry(textMinimalWidth, padding, sliderWidth, height()); 31 | } 32 | 33 | } // end namespace rviz 34 | -------------------------------------------------------------------------------- /free_gait_rviz_plugin/src/rviz/properties/line_edit_with_slider.h: -------------------------------------------------------------------------------- 1 | /* 2 | * line_edit_with_slider.h 3 | * 4 | * Created on: Dec 12, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | class QSlider; 14 | 15 | namespace rviz { 16 | 17 | /** 18 | * A QLineEdit for a numeric value with a slider to the right. 19 | */ 20 | class LineEditWithSlider : public QLineEdit 21 | { 22 | Q_OBJECT 23 | public: 24 | LineEditWithSlider(QWidget* parent = 0); 25 | 26 | /** Returns the child slider. Use this to connect() something to a 27 | * changed value. */ 28 | QSlider* slider() { return slider_; } 29 | 30 | protected: 31 | virtual void resizeEvent(QResizeEvent* event); 32 | 33 | private: 34 | QSlider* slider_; 35 | }; 36 | 37 | } // end namespace rviz 38 | -------------------------------------------------------------------------------- /free_gait_rviz_plugin/test/class_load_test.cpp: -------------------------------------------------------------------------------- 1 | #include "free_gait_core/free_gait_core.hpp" 2 | #include "free_gait_ros/free_gait_ros.hpp" 3 | #include "free_gait_rviz_plugin/FreeGaitPreviewDisplay.hpp" 4 | #include "pluginlib_tutorials/polygon_plugins.h" 5 | #include "pluginlib_tutorials/polygon_base.h" 6 | using namespace free_gait_rviz_plugin; 7 | using namespace free_gait; 8 | namespace test_load_plugin { 9 | class TestLoad 10 | { 11 | public: 12 | TestLoad(ros::NodeHandle& nh) : nh_(nh), 13 | AdapterRos_(nh_, free_gait::AdapterRos::AdapterType::Preview) {} 14 | ~TestLoad(){} 15 | private: 16 | ros::NodeHandle nh_; 17 | AdapterRos AdapterRos_; 18 | 19 | }; 20 | }; 21 | 22 | int main(int argc, char *argv[]) 23 | { 24 | ros::init(argc, argv, "class_load_test"); 25 | ros::NodeHandle nh("~"); 26 | test_load_plugin::TestLoad TL(nh); 27 | pluginlib::ClassLoader tutor_loader("pluginlib_tutorials", "polygon_base::RegularPolygon"); 28 | tutor_loader.createInstance("pluginlib_tutorials/regular_triangle"); 29 | // AdapterRos adapterRos_(nh, free_gait::AdapterRos::AdapterType::Preview); 30 | // pluginlib::ClassLoader adapterLoader("free_gait_core", "free_gait::AdapterBase"); 31 | pluginlib::ClassLoader* display_loader; 32 | display_loader = new pluginlib::ClassLoader("rviz", "rviz::Display"); 33 | // std::unique_ptr adapter_loader; 34 | // std::unique_ptr display; 35 | // display.reset(display_loader.createUnmanagedInstance("free_gait_rviz_plugin/FreeGaitPreviewDisplay")); 36 | // adapter_loader.reset(adapterLoader.createUnmanagedInstance("free_gait::AdapterDummy")); 37 | display_loader->createUnmanagedInstance("free_gait_rviz_plugin/FreeGaitPreview"); 38 | return 0; 39 | } 40 | -------------------------------------------------------------------------------- /my_actions/actions/test.yaml: -------------------------------------------------------------------------------- 1 | actions: 2 | - action: 3 | id: "1" 4 | name: sit down 5 | file: motion_scripts/sitdown.yaml 6 | type: yaml 7 | description: robot sit down 8 | - action: 9 | id: "2" 10 | name: stand up 11 | file: motion_scripts/standup.yaml 12 | type: yaml 13 | description: robot stand 14 | - action: 15 | id: "3" 16 | name: walk a cycle 17 | file: motion_scripts/stational_walk.yaml 18 | type: yaml 19 | description: robot walk a cycle 20 | - action: 21 | id: "4" 22 | name: base_target demo 23 | file: motion_scripts/base_target_demo.yaml 24 | type: yaml 25 | description: robot base move a rectangle 26 | - action: 27 | id: "5" 28 | name: end effector target demo 29 | file: motion_scripts/end_effectors_target_demo.yaml 30 | type: yaml 31 | description: robot move leg end effector 32 | - action: 33 | id: "6" 34 | name: base pitch rotate 35 | file: motion_scripts/base_rotate_pitch.yaml 36 | type: yaml 37 | description: base pitch rotate 20 degree 38 | - action: 39 | id: "7" 40 | name: base yaw rotate 41 | file: motion_scripts/base_rotate_yaw.yaml 42 | type: yaml 43 | description: base yaw rotate 20 degree 44 | - action: 45 | id: "8" 46 | name: base roll rotate 47 | file: motion_scripts/base_rotate_roll.yaml 48 | type: yaml 49 | description: base roll rotate 20 degree 50 | - action: 51 | id: "9" 52 | name: base pitch and yaw rotate 53 | file: motion_scripts/base_rotate_yaw_pitch.yaml 54 | type: yaml 55 | description: base yaw and pitch rotate 56 | - action: 57 | id: "10" 58 | name: pace in Gazebo 59 | file: motion_scripts/pace_gazebo.yaml 60 | type: yaml 61 | description: a cycle of pace motion in gazebo 62 | - action: 63 | id: "11" 64 | name: trot in Gazebo 65 | file: motion_scripts/trot_test.yaml 66 | type: yaml 67 | description: a cycle of trot motion in gazebo 68 | - action: 69 | id: "12" 70 | name: change to spot 71 | file: motion_scripts/change_to_spot.yaml 72 | type: yaml 73 | description: change the leg configuration to a "Spot" like 74 | - action: 75 | id: "13" 76 | name: Leg move Demo 77 | file: motion_scripts/leg_move.yaml 78 | type: yaml 79 | description: move four legs 80 | -------------------------------------------------------------------------------- /my_actions/collections/test_collections.yaml: -------------------------------------------------------------------------------- 1 | collections: 2 | - collection: 3 | id: "1" 4 | name: collection_1 5 | actions: 6 | - "1" 7 | - "2" 8 | is_sequence: True 9 | - collection: 10 | id: "2" 11 | name: walk_demo 12 | actions: 13 | - "1" 14 | - "2" 15 | - "3" 16 | is_sequence: True 17 | -------------------------------------------------------------------------------- /my_actions/motion_scripts/base_rotate_pitch.yaml: -------------------------------------------------------------------------------- 1 | adapt_coordinates: 2 | - transform: 3 | source_frame: base_link 4 | target_frame: odom 5 | steps: 6 | - step: 7 | - base_auto: 8 | height: 0.3 9 | ignore_timing_of_leg_motion: false 10 | - step: 11 | - base_target: 12 | ignore_timing_of_leg_motion: false 13 | average_linear_velocity: 0.1 14 | average_angular_velocity: 0.2 15 | target: 16 | frame: base_link 17 | position: [0.01,0.0,0.0] 18 | orientation: [0.0,0.3,0.0] 19 | - step: 20 | - base_target: 21 | ignore_timing_of_leg_motion: false 22 | average_linear_velocity: 0.1 23 | average_angular_velocity: 0.2 24 | target: 25 | frame: base_link 26 | position: [0.01,0.0,0.0] 27 | orientation: [0.0,-0.3,0.0] 28 | - step: 29 | - base_auto: 30 | height: 0.3 31 | ignore_timing_of_leg_motion: false 32 | -------------------------------------------------------------------------------- /my_actions/motion_scripts/base_rotate_roll.yaml: -------------------------------------------------------------------------------- 1 | adapt_coordinates: 2 | - transform: 3 | source_frame: base_link 4 | target_frame: odom 5 | steps: 6 | - step: 7 | - base_auto: 8 | height: 0.3 9 | ignore_timing_of_leg_motion: false 10 | - step: 11 | - base_target: 12 | ignore_timing_of_leg_motion: false 13 | average_linear_velocity: 0.1 14 | average_angular_velocity: 0.2 15 | target: 16 | frame: base_link 17 | position: [0.01,0.0,0.0] 18 | orientation: [0.3,0.0,0.0] 19 | - step: 20 | - base_target: 21 | ignore_timing_of_leg_motion: false 22 | average_linear_velocity: 0.1 23 | average_angular_velocity: 0.2 24 | target: 25 | frame: base_link 26 | position: [0.01,0.0,0.0] 27 | orientation: [-0.3,0.0,0.0] 28 | - step: 29 | - base_auto: 30 | height: 0.3 31 | ignore_timing_of_leg_motion: false 32 | -------------------------------------------------------------------------------- /my_actions/motion_scripts/base_rotate_yaw.yaml: -------------------------------------------------------------------------------- 1 | adapt_coordinates: 2 | - transform: 3 | source_frame: base_link 4 | target_frame: odom 5 | steps: 6 | - step: 7 | - base_auto: 8 | height: 0.3 9 | ignore_timing_of_leg_motion: false 10 | - step: 11 | - base_target: 12 | ignore_timing_of_leg_motion: false 13 | average_linear_velocity: 0.1 14 | average_angular_velocity: 0.2 15 | target: 16 | frame: base_link 17 | position: [0.01,0.0,0.0] 18 | orientation: [0.0,0.0,0.4] 19 | - step: 20 | - base_target: 21 | ignore_timing_of_leg_motion: false 22 | average_linear_velocity: 0.1 23 | average_angular_velocity: 0.2 24 | target: 25 | frame: base_link 26 | position: [0.01,0.0,0.0] 27 | orientation: [0.0,0.0,0.4] 28 | - step: 29 | - base_auto: 30 | height: 0.3 31 | ignore_timing_of_leg_motion: false 32 | -------------------------------------------------------------------------------- /my_actions/motion_scripts/base_rotate_yaw_pitch.yaml: -------------------------------------------------------------------------------- 1 | adapt_coordinates: 2 | - transform: 3 | source_frame: base_link 4 | target_frame: odom 5 | steps: 6 | - step: 7 | - base_auto: 8 | height: 0.3 9 | ignore_timing_of_leg_motion: false 10 | - step: 11 | - base_target: 12 | ignore_timing_of_leg_motion: false 13 | average_linear_velocity: 0.1 14 | average_angular_velocity: 0.2 15 | target: 16 | frame: base_link 17 | position: [0.01,0.0,0.0] 18 | orientation: [0.0,0.3,0.3] 19 | - step: 20 | - base_target: 21 | ignore_timing_of_leg_motion: false 22 | average_linear_velocity: 0.1 23 | average_angular_velocity: 0.2 24 | target: 25 | frame: base_link 26 | position: [0.01,0.0,0.0] 27 | orientation: [0.0,0.3,-0.3] 28 | - step: 29 | - base_auto: 30 | height: 0.3 31 | ignore_timing_of_leg_motion: false 32 | -------------------------------------------------------------------------------- /my_actions/motion_scripts/base_target_demo.yaml: -------------------------------------------------------------------------------- 1 | adapt_coordinates: 2 | - transform: 3 | source_frame: base_link 4 | target_frame: odom 5 | steps: 6 | - step: 7 | - base_auto: 8 | height: 0.4 9 | ignore_timing_of_leg_motion: false 10 | - step: 11 | - base_target: 12 | ignore_timing_of_leg_motion: false 13 | average_linear_velocity: 0.1 14 | average_angular_velocity: 0.02 15 | target: 16 | frame: base_link 17 | position: [0.1,0.0,0.0] 18 | orientation: [0.0,0.0,0.0,1.0] 19 | - step: 20 | - base_target: 21 | target: 22 | frame: base_link 23 | position: [0.1,-0.1,0.0] 24 | orientation: [0.0,0.0,0.0,1.0] 25 | ignore_timing_of_leg_motion: false 26 | average_linear_velocity: 0.1 27 | average_angular_velocity: 0.02 28 | - step: 29 | - base_target: 30 | target: 31 | frame: base_link 32 | position: [-0.1,-0.1,0.0] 33 | orientation: [0.0,0.0,0.0,1.0] 34 | ignore_timing_of_leg_motion: false 35 | average_linear_velocity: 0.1 36 | average_angular_velocity: 0.02 37 | - step: 38 | - base_target: 39 | target: 40 | frame: base_link 41 | position: [-0.1,0.1,0.0] 42 | orientation: [0.0,0.0,0.0,1.0] 43 | ignore_timing_of_leg_motion: false 44 | average_linear_velocity: 0.1 45 | average_angular_velocity: 0.02 46 | - step: 47 | - base_target: 48 | target: 49 | frame: base_link 50 | position: [0.01,0.01,0.0] 51 | orientation: [0.0,0.0,0.0,1.0] 52 | ignore_timing_of_leg_motion: false 53 | average_linear_velocity: 0.1 54 | average_angular_velocity: 0.02 55 | -------------------------------------------------------------------------------- /my_actions/motion_scripts/change_to_spot.yaml: -------------------------------------------------------------------------------- 1 | adapt_coordinates: 2 | - transform: 3 | source_frame: base_link 4 | target_frame: odom 5 | steps: 6 | - step: 7 | - base_auto: 8 | height: 0.1 9 | ignore_timing_of_leg_motion: false 10 | - step: 11 | - base_target: 12 | ignore_timing_of_leg_motion: false 13 | average_linear_velocity: 0.1 14 | average_angular_velocity: 0.2 15 | target: 16 | frame: base_link 17 | position: [0.0,0.0,-0.3] 18 | orientation: [0.0,0.0,0.0,1.0] 19 | 20 | 21 | - step: 22 | - joint_trajectory: 23 | name: RH_LEG 24 | trajectory: 25 | joint_names: [RH_LEG_HAA, RH_LEG_HFE, RH_LEG_KFE] 26 | knots: [{time: 0.5, positions: [0,1.57,-3.14]},{time: 1.5, positions: [0,4.54,-3.53]}] 27 | ignore_contact: false 28 | - joint_trajectory: 29 | name: LH_LEG 30 | trajectory: 31 | joint_names: [LH_LEG_HAA, LH_LEG_HFE, LH_LEG_KFE] 32 | knots: [{time: 0.5, positions: [0,-1.57,3.14]},{time: 1.5, positions: [0,-4.54,3.53]}] 33 | ignore_contact: false 34 | - step: 35 | - leg_mode: 36 | name: RH_LEG 37 | support_leg: true 38 | surface_normal: [0,0,1] 39 | ignore_for_pose_adaptation: false 40 | - leg_mode: 41 | name: LH_LEG 42 | support_leg: true 43 | surface_normal: [0,0,1] 44 | ignore_for_pose_adaptation: false 45 | -------------------------------------------------------------------------------- /my_actions/motion_scripts/end_effectors_target_demo.yaml: -------------------------------------------------------------------------------- 1 | adapt_coordinates: 2 | - transform: 3 | source_frame: foot_print 4 | target_frame: odom 5 | steps: 6 | - step: 7 | - base_auto: 8 | height: 0.4 9 | ignore_timing_of_leg_motion: false 10 | - step: 11 | - end_effector_target: 12 | name: LF_LEG 13 | ignore_contact: true 14 | ignore_for_pose_adaptation: true 15 | target_position: 16 | frame: foot_print 17 | position: [0.4,0.25,0.2] 18 | - step: 19 | - base_auto: 20 | height: 0.25 21 | ignore_timing_of_leg_motion: false 22 | - end_effector_target: 23 | name: LF_LEG 24 | ignore_contact: true 25 | ignore_for_pose_adaptation: true 26 | target_position: 27 | frame: foot_print 28 | position: [0.4,0.25,0.2] 29 | - step: 30 | - base_auto: 31 | height: 0.4 32 | ignore_timing_of_leg_motion: false 33 | - step: 34 | - footstep: 35 | name: LF_LEG 36 | profile_type: straight 37 | target: 38 | frame: foot_print 39 | position: [0.4,0.25,0] 40 | - step: 41 | - base_auto: 42 | height: 0.4 43 | ignore_timing_of_leg_motion: true 44 | -------------------------------------------------------------------------------- /my_actions/motion_scripts/leg_move.yaml: -------------------------------------------------------------------------------- 1 | adapt_coordinates: 2 | - transform: 3 | source_frame: foot_print 4 | target_frame: odom 5 | 6 | steps: 7 | - step: 8 | - footstep: 9 | name: LF_LEG 10 | profile_type: triangle 11 | profile_height: 0.10 12 | average_velocity: 0.15 13 | target: 14 | frame: foot_print 15 | position: [0.42, 0.25, 0] 16 | - footstep: 17 | name: RH_LEG 18 | profile_type: triangle 19 | profile_height: 0.10 20 | average_velocity: 0.15 21 | target: 22 | frame: foot_print 23 | position: [-0.42, -0.25, 0] 24 | - footstep: 25 | name: RF_LEG 26 | profile_type: foot_print 27 | profile_height: 0.10 28 | average_velocity: 0.15 29 | target: 30 | frame: foot_print 31 | position: [0.42, -0.25, 0] 32 | - footstep: 33 | name: LH_LEG 34 | profile_type: triangle 35 | profile_height: 0.10 36 | average_velocity: 0.15 37 | target: 38 | frame: foot_print 39 | position: [-0.42, 0.25, 0] 40 | - step: 41 | - footstep: 42 | name: LF_LEG 43 | profile_type: triangle 44 | profile_height: 0.10 45 | average_velocity: 0.15 46 | target: 47 | frame: foot_print 48 | position: [0.5, 0.25, 0] 49 | - step: 50 | - footstep: 51 | name: RH_LEG 52 | profile_type: triangle 53 | profile_height: 0.10 54 | average_velocity: 0.15 55 | target: 56 | frame: foot_print 57 | position: [-0.3, -0.25, 0] 58 | - step: 59 | - footstep: 60 | name: RF_LEG 61 | profile_type: foot_print 62 | profile_height: 0.10 63 | average_velocity: 0.15 64 | target: 65 | frame: foot_print 66 | position: [0.5, -0.25, 0] 67 | - step: 68 | - footstep: 69 | name: LH_LEG 70 | profile_type: triangle 71 | profile_height: 0.10 72 | average_velocity: 0.15 73 | target: 74 | frame: foot_print 75 | position: [-0.3, 0.25, 0] 76 | -------------------------------------------------------------------------------- /my_actions/motion_scripts/pace_gazebo.yaml: -------------------------------------------------------------------------------- 1 | adapt_coordinates: 2 | - transform: 3 | source_frame: base_link 4 | target_frame: odom 5 | 6 | steps: 7 | - step: 8 | - base_auto: 9 | height: 0.4 10 | ignore_timing_of_leg_motion: true 11 | average_linear_velocity: 0.5 12 | average_angular_velocity: 0.5 13 | - step: 14 | - footstep: 15 | name: LF_LEG 16 | profile_type: triangle 17 | profile_height: 0.10 18 | average_velocity: 0.15 19 | target: 20 | frame: base_link 21 | position: [0.5, 0.25, -0.4] 22 | - step: 23 | - base_auto: 24 | height: 0.4 25 | ignore_timing_of_leg_motion: true 26 | average_linear_velocity: 0.5 27 | average_angular_velocity: 0.5 28 | - step: 29 | - footstep: 30 | name: RH_LEG 31 | profile_type: triangle 32 | profile_height: 0.10 33 | average_velocity: 0.15 34 | target: 35 | frame: base_link 36 | position: [-0.3, -0.25, -0.4] 37 | - step: 38 | - base_auto: 39 | height: 0.4 40 | ignore_timing_of_leg_motion: true 41 | average_linear_velocity: 0.5 42 | average_angular_velocity: 0.5 43 | - step: 44 | - footstep: 45 | name: RF_LEG 46 | profile_type: triangle 47 | profile_height: 0.10 48 | average_velocity: 0.15 49 | target: 50 | frame: base_link 51 | position: [0.5, -0.25, -0.4] 52 | - step: 53 | - base_auto: 54 | height: 0.4 55 | ignore_timing_of_leg_motion: true 56 | average_linear_velocity: 0.5 57 | average_angular_velocity: 0.5 58 | - step: 59 | - footstep: 60 | name: LH_LEG 61 | profile_type: triangle 62 | profile_height: 0.10 63 | average_velocity: 0.15 64 | target: 65 | frame: base_link 66 | position: [-0.3, 0.25, -0.4] 67 | -------------------------------------------------------------------------------- /my_actions/motion_scripts/sitdown.yaml: -------------------------------------------------------------------------------- 1 | steps: 2 | - step: 3 | - base_auto: 4 | height: 0.15 5 | ignore_timing_of_leg_motion: false 6 | -------------------------------------------------------------------------------- /my_actions/motion_scripts/standup.yaml: -------------------------------------------------------------------------------- 1 | steps: 2 | - step: 3 | - base_auto: 4 | height: 0.5 5 | ignore_timing_of_leg_motion: false 6 | -------------------------------------------------------------------------------- /my_actions/motion_scripts/stational_walk.yaml: -------------------------------------------------------------------------------- 1 | adapt_coordinates: 2 | - transform: 3 | source_frame: foot_print 4 | target_frame: odom 5 | 6 | steps: 7 | - step: 8 | - base_auto: 9 | height: 0.4 10 | ignore_timing_of_leg_motion: false 11 | average_linear_velocity: 0.5 12 | average_angular_velocity: 0.5 13 | - footstep: 14 | name: LF_LEG 15 | profile_type: triangle 16 | profile_height: 0.10 17 | average_velocity: 0.15 18 | target: 19 | frame: foot_print 20 | position: [0.5, 0.25, 0] 21 | - step: 22 | - base_auto: 23 | height: 0.4 24 | ignore_timing_of_leg_motion: false 25 | average_linear_velocity: 0.5 26 | average_angular_velocity: 0.5 27 | - footstep: 28 | name: RH_LEG 29 | profile_type: triangle 30 | profile_height: 0.10 31 | average_velocity: 0.15 32 | target: 33 | frame: foot_print 34 | position: [-0.3, -0.25, 0] 35 | - step: 36 | - base_auto: 37 | height: 0.4 38 | ignore_timing_of_leg_motion: false 39 | average_linear_velocity: 0.5 40 | average_angular_velocity: 0.5 41 | - footstep: 42 | name: RF_LEG 43 | profile_type: foot_print 44 | profile_height: 0.10 45 | average_velocity: 0.15 46 | target: 47 | frame: foot_print 48 | position: [0.5, -0.25, 0] 49 | - step: 50 | - base_auto: 51 | height: 0.4 52 | ignore_timing_of_leg_motion: false 53 | average_linear_velocity: 0.5 54 | average_angular_velocity: 0.5 55 | - footstep: 56 | name: LH_LEG 57 | profile_type: triangle 58 | profile_height: 0.10 59 | average_velocity: 0.15 60 | target: 61 | frame: foot_print 62 | position: [-0.3, 0.25, 0] 63 | -------------------------------------------------------------------------------- /my_actions/motion_scripts/trot_test.yaml: -------------------------------------------------------------------------------- 1 | adapt_coordinates: 2 | - transform: 3 | source_frame: foot_print 4 | target_frame: odom 5 | 6 | steps: 7 | - step: 8 | - footstep: 9 | name: LF_LEG 10 | profile_type: triangle 11 | profile_height: 0.10 12 | average_velocity: 1 13 | target: 14 | frame: foot_print 15 | position: [0.4, 0.25, 0] 16 | - footstep: 17 | name: RH_LEG 18 | profile_type: triangle 19 | profile_height: 0.10 20 | average_velocity: 1 21 | target: 22 | frame: foot_print 23 | position: [-0.4, -0.25, 0] 24 | - step: 25 | - footstep: 26 | name: RF_LEG 27 | profile_type: triangle 28 | profile_height: 0.10 29 | average_velocity: 1 30 | target: 31 | frame: foot_print 32 | position: [0.4, -0.25, 0] 33 | - footstep: 34 | name: LH_LEG 35 | profile_type: triangle 36 | profile_height: 0.10 37 | average_velocity: 1 38 | target: 39 | frame: foot_print 40 | position: [-0.4, 0.25, 0] 41 | -------------------------------------------------------------------------------- /qp_solver/include/qp_solver/pose_optimization/PoseConstraintsChecker.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PoseConstraintsChecker.hpp 3 | * 4 | * Created on: Aug 31, 2017 5 | * Author: Péter Fankhauser 6 | */ 7 | 8 | #pragma once 9 | 10 | #include "qp_solver/pose_optimization/PoseOptimizationBase.hpp" 11 | #include "free_gait_core/TypeDefs.hpp" 12 | #include "free_gait_core/executor/AdapterBase.hpp" 13 | 14 | #include 15 | 16 | namespace free_gait { 17 | 18 | class PoseConstraintsChecker : public PoseOptimizationBase 19 | { 20 | public: 21 | PoseConstraintsChecker(const AdapterBase& adapter); 22 | virtual ~PoseConstraintsChecker(); 23 | 24 | void setTolerances(const double centerOfMassTolerance, const double legLengthTolerance); 25 | 26 | bool check(const Pose& pose); 27 | 28 | private: 29 | double centerOfMassTolerance_; 30 | double legLengthTolerance_; 31 | }; 32 | 33 | } 34 | -------------------------------------------------------------------------------- /qp_solver/include/qp_solver/pose_optimization/PoseOptimizationGeometric.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Geometric.hpp 3 | * 4 | * Created on: Aug 30, 2017 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "qp_solver/pose_optimization/PoseOptimizationBase.hpp" 12 | #include "free_gait_core/TypeDefs.hpp" 13 | 14 | namespace free_gait { 15 | 16 | class PoseOptimizationGeometric : public PoseOptimizationBase 17 | { 18 | public: 19 | PoseOptimizationGeometric(const AdapterBase& adapter); 20 | virtual ~PoseOptimizationGeometric(); 21 | 22 | /*! 23 | * Set the positions of the feet of the robot in world coordinate 24 | * system for the computation of the orientation. 25 | * @param stance the position to determine the orientation. 26 | */ 27 | void setStanceForOrientation(const Stance& stance); 28 | 29 | /*! 30 | * Computes the optimized pose. 31 | * @param stanceForOrientation the positions of the feet of the robot in world coordinate 32 | * system for the computation of the orientation. 33 | * @return the optimized pose. 34 | */ 35 | bool optimize(Pose& pose); 36 | 37 | private: 38 | Stance stanceForOrientation_; 39 | }; 40 | 41 | } /* namespace loco */ 42 | -------------------------------------------------------------------------------- /qp_solver/include/qp_solver/pose_optimization/PoseOptimizationProblem.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PoseOptimizationProblem.hpp 3 | * 4 | * Created on: Mar 23, 2017 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "qp_solver/pose_optimization/PoseOptimizationObjectiveFunction.hpp" 12 | #include "qp_solver/pose_optimization/PoseOptimizationFunctionConstraints.hpp" 13 | 14 | //#include 15 | 16 | namespace free_gait { 17 | 18 | class PoseOptimizationProblem //: public numopt_common::ConstrainedNonlinearProblem 19 | { 20 | public: 21 | PoseOptimizationProblem(std::shared_ptr objectiveFunction, 22 | std::shared_ptr functionConstraints); 23 | virtual ~PoseOptimizationProblem(); 24 | std::shared_ptr objectiveFunction_; 25 | std::shared_ptr functionConstraints_; 26 | }; 27 | 28 | } /* namespace */ 29 | -------------------------------------------------------------------------------- /qp_solver/include/qp_solver/pose_optimization/PoseOptimizationQP.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PoseOptimizationQP.hpp 3 | * 4 | * Created on: Jun 10, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_core/TypeDefs.hpp" 12 | #include "qp_solver/pose_optimization/PoseOptimizationBase.hpp" 13 | 14 | #include 15 | #include 16 | 17 | 18 | #include 19 | #include 20 | 21 | 22 | 23 | namespace free_gait { 24 | 25 | class PoseOptimizationQP : public PoseOptimizationBase 26 | { 27 | public: 28 | PoseOptimizationQP(const AdapterBase& adapter); 29 | virtual ~PoseOptimizationQP(); 30 | // PoseOptimizationQP(const PoseOptimizationQP& other); 31 | 32 | /*! 33 | * Computes the optimized pose. 34 | * @param[in/out] pose the pose to optimize from the given initial guess. 35 | * @return true if successful, false otherwise. 36 | */ 37 | bool optimize(Pose& pose); 38 | 39 | private: 40 | std::unique_ptr solver_; 41 | unsigned int nStates_; 42 | unsigned int nDimensions_; 43 | }; 44 | 45 | } /* namespace loco */ 46 | -------------------------------------------------------------------------------- /qp_solver/include/qp_solver/pose_optimization/pose_optimization.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * pose_optimization.hpp 3 | * 4 | * Created on: Oct 19, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "qp_solver/pose_optimization/PoseConstraintsChecker.hpp" 12 | #include "qp_solver/pose_optimization/PoseOptimizationGeometric.hpp" 13 | #include "qp_solver/pose_optimization/PoseOptimizationQP.hpp" 14 | #include "qp_solver/pose_optimization/PoseOptimizationSQP.hpp" 15 | -------------------------------------------------------------------------------- /qp_solver/include/qp_solver/pose_optimization/poseparameterization.h: -------------------------------------------------------------------------------- 1 | /* 2 | * PoseParameterization.hpp 3 | * 4 | * Created on: Mar 22, 2017 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "free_gait_core/TypeDefs.hpp" 12 | 13 | #include "qp_solver/quadraticproblemsolver.h" 14 | 15 | namespace free_gait { 16 | 17 | class PoseParameterization //: public numopt_common::Parameterization 18 | { 19 | public: 20 | PoseParameterization(); 21 | virtual ~PoseParameterization(); 22 | 23 | // PoseParameterization(const PoseParameterization& other); 24 | 25 | qp_solver::QuadraticProblemSolver::parameters& getParams(); 26 | const qp_solver::QuadraticProblemSolver::parameters& getParams() const; 27 | 28 | bool plus(qp_solver::QuadraticProblemSolver::parameters& result, 29 | const qp_solver::QuadraticProblemSolver::parameters& p, 30 | const qp_solver::QuadraticProblemSolver::Delta& dp) const; 31 | 32 | bool getTransformMatrixLocalToGlobal(Eigen::MatrixXd& matrix, 33 | const qp_solver::QuadraticProblemSolver::parameters& params) const; 34 | 35 | bool getTransformMatrixGlobalToLocal(Eigen::MatrixXd& matrix, 36 | const qp_solver::QuadraticProblemSolver::parameters& params) const; 37 | 38 | int getGlobalSize() const; 39 | static const size_t getGlobalSizeStatic(); 40 | int getLocalSize() const; 41 | 42 | bool setRandom(qp_solver::QuadraticProblemSolver::parameters& p) const; 43 | bool setIdentity(qp_solver::QuadraticProblemSolver::parameters& p) const; 44 | 45 | // Parameterization* clone() const; 46 | 47 | const Pose getPose() const; 48 | void setPose(const Pose& pose); 49 | const Position getPosition() const; 50 | const RotationQuaternion getOrientation() const; 51 | 52 | private: 53 | const static size_t nTransGlobal_ = 3; 54 | const static size_t nRotGlobal_ = 4; 55 | const static size_t nTransLocal_ = 3; 56 | const static size_t nRotLocal_ = 3; 57 | 58 | //! Global state vector with position and orientation. 59 | qp_solver::QuadraticProblemSolver::parameters params_; 60 | }; 61 | 62 | } /* namespace free_gait */ 63 | -------------------------------------------------------------------------------- /qp_solver/include/qp_solver/sequencequadraticproblemsolver.h: -------------------------------------------------------------------------------- 1 | #ifndef SEQUENCEQUADRATICPROBLEMSOLVER_H 2 | #define SEQUENCEQUADRATICPROBLEMSOLVER_H 3 | #include "qp_solver/quadraticproblemsolver.h" 4 | #include "free_gait_core/pose_optimization/PoseOptimizationProblem.hpp" 5 | #include "free_gait_core/pose_optimization/PoseOptimizationFunctionConstraints.hpp" 6 | #include "free_gait_core/pose_optimization/PoseOptimizationObjectiveFunction.hpp" 7 | #include "free_gait_core/pose_optimization/poseparameterization.h" 8 | #include 9 | namespace sqp_solver { 10 | using namespace free_gait; 11 | class QuadraticProblemSolver; 12 | class QuadraticObjectiveFunction; 13 | class LinearFunctionConstraints; 14 | 15 | class SequenceQuadraticProblemSolver 16 | { 17 | public: 18 | SequenceQuadraticProblemSolver(std::shared_ptr& quadratic_solver, 19 | double tolerance, int max_iteration); 20 | virtual ~SequenceQuadraticProblemSolver(); 21 | 22 | bool minimize(const PoseOptimizationProblem& problem, 23 | PoseParameterization& params); 24 | 25 | private: 26 | double tolerance_; 27 | int max_iteration_; 28 | std::shared_ptr quadratic_solver_; 29 | }; 30 | 31 | }// namespace sqp_solver 32 | #endif // SEQUENCEQUADRATICPROBLEMSOLVER_H 33 | -------------------------------------------------------------------------------- /qp_solver/src/Array.cc: -------------------------------------------------------------------------------- 1 | // $Id: Array.cc 201 2008-05-18 19:47:38Z digasper $ 2 | // This file is part of QuadProg++: 3 | // Copyright (C) 2006--2009 Luca Di Gaspero. 4 | // 5 | // This software may be modified and distributed under the terms 6 | // of the MIT license. See the LICENSE file for details. 7 | 8 | #include "qp_solver/qp_array.h" 9 | 10 | /** 11 | Index utilities 12 | */ 13 | 14 | namespace quadprogpp { 15 | 16 | std::set seq(unsigned int s, unsigned int e) 17 | { 18 | std::set tmp; 19 | for (unsigned int i = s; i <= e; i++) 20 | tmp.insert(i); 21 | 22 | return tmp; 23 | } 24 | 25 | std::set singleton(unsigned int i) 26 | { 27 | std::set tmp; 28 | tmp.insert(i); 29 | 30 | return tmp; 31 | } 32 | 33 | } // namespace quadprogpp 34 | -------------------------------------------------------------------------------- /qp_solver/src/TypeDefs.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TypeDefs.cpp 3 | * 4 | * Created on: Jan 6, 2017 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #include "free_gait_core/TypeDefs.hpp" 10 | 11 | namespace free_gait { 12 | 13 | struct CompareByCounterClockwiseOrder 14 | { 15 | bool operator()(const LimbEnum& a, const LimbEnum& b) const 16 | { 17 | const size_t aIndex = findIndex(a); 18 | const size_t bIndex = findIndex(b); 19 | return aIndex < bIndex; 20 | } 21 | 22 | size_t findIndex(const LimbEnum& limb) const 23 | { 24 | auto it = std::find(limbEnumCounterClockWiseOrder.begin(), limbEnumCounterClockWiseOrder.end(), limb); 25 | if (it == limbEnumCounterClockWiseOrder.end()) { 26 | throw std::runtime_error("Could not find index for limb!"); 27 | } 28 | return std::distance(limbEnumCounterClockWiseOrder.begin(), it); 29 | } 30 | }; 31 | 32 | void getFootholdsCounterClockwiseOrdered(const Stance& stance, std::vector& footholds) 33 | { 34 | footholds.reserve(stance.size()); 35 | std::map stanceOrdered; 36 | for (const auto& limb : stance) stanceOrdered.insert(limb); 37 | for (const auto& limb : stanceOrdered) footholds.push_back(limb.second); 38 | }; 39 | 40 | } // namespace 41 | -------------------------------------------------------------------------------- /qp_solver/src/pose_optimization/PoseConstraintsChecker.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PoseConstraintsChecker.cpp 3 | * 4 | * Created on: Aug 31, 2017 5 | * Author: Péter Fankhauser 6 | */ 7 | 8 | #include "qp_solver/pose_optimization/PoseConstraintsChecker.hpp" 9 | 10 | namespace free_gait { 11 | 12 | PoseConstraintsChecker::PoseConstraintsChecker(const AdapterBase& adapter) 13 | : PoseOptimizationBase(adapter), 14 | centerOfMassTolerance_(0.0), 15 | legLengthTolerance_(0.0) 16 | { 17 | } 18 | 19 | PoseConstraintsChecker::~PoseConstraintsChecker() 20 | { 21 | } 22 | 23 | void PoseConstraintsChecker::setTolerances(const double centerOfMassTolerance, const double legLengthTolerance) 24 | { 25 | centerOfMassTolerance_ = centerOfMassTolerance; 26 | legLengthTolerance_ = legLengthTolerance; 27 | } 28 | 29 | bool PoseConstraintsChecker::check(const Pose& pose) 30 | { 31 | // TODO(Shunyao): findout the useness of those lines 32 | //! state_ is a protected member in poseOptimizationBase, set it to AdapterBase, 33 | //! implemented in AdapterDummy 34 | state_.setPoseBaseToWorld(pose); // this member function is in the quadrupedModel class, which is a parent class of State 35 | // adapter_.setInternalDataFromState(state_, false, true, false, false); // To guide IK. 36 | adapter_.setInternalDataFromState(state_); // To guide IK. 37 | // if (!updateJointPositionsInState(state_)) { 38 | // return false; 39 | // } 40 | // adapter_.setInternalDataFromState(state_, false, true, false, false); 41 | 42 | // Check center of mass. 43 | grid_map::Polygon supportRegionCopy(supportRegion_); 44 | supportRegionCopy.offsetInward(centerOfMassTolerance_); 45 | if (!supportRegion_.isInside(adapter_.getCenterOfMassInWorldFrame().vector().head(2))) { 46 | std::cout<<"Center of mass: "< maxLimbLenghts_[foot.first] + legLengthTolerance_) { 56 | std::cout<<"Leg lenth is out of limits"<()); 57 | } 58 | } 59 | /** 60 | * @brief PoseOptimizationBase::updateJointPositionsInState 61 | * @param state 62 | * @return 63 | */ 64 | bool PoseOptimizationBase::updateJointPositionsInState(State& state) const 65 | { 66 | bool totalSuccess = true; 67 | for (const auto& foot : stance_) { 68 | const Position footPositionInBase( 69 | adapter_.transformPosition(adapter_.getWorldFrameId(), adapter_.getBaseFrameId(), foot.second)); 70 | JointPositionsLeg jointPositions; 71 | const bool success = adapter_.getLimbJointPositionsFromPositionBaseToFootInBaseFrame(footPositionInBase, foot.first, jointPositions); 72 | //TODO(Shunyao): fix state feedback 73 | // if (success) state.setJointPositionsForLimb(foot.first, jointPositions); 74 | // else totalSuccess = totalSuccess && success; 75 | } 76 | return totalSuccess; 77 | } 78 | 79 | } 80 | -------------------------------------------------------------------------------- /qp_solver/src/pose_optimization/PoseOptimizationProblem.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PoseOptimizationProblem.cpp 3 | * 4 | * Created on: Mar 23, 2017 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich 7 | */ 8 | #include "qp_solver/pose_optimization/PoseOptimizationProblem.hpp" 9 | 10 | namespace free_gait { 11 | 12 | PoseOptimizationProblem::PoseOptimizationProblem( 13 | std::shared_ptr objectiveFunction, 14 | std::shared_ptr functionConstraints) 15 | //: ConstrainedNonlinearProblem(objectiveFunction, functionConstraints) 16 | : objectiveFunction_(objectiveFunction), 17 | functionConstraints_(functionConstraints) 18 | { 19 | } 20 | 21 | PoseOptimizationProblem::~PoseOptimizationProblem() 22 | { 23 | } 24 | 25 | } /* namespace */ 26 | -------------------------------------------------------------------------------- /qp_solver/test/FootstepTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FootstepTest.cpp 3 | * 4 | * Created on: Feb 11, 2017 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #include "free_gait_core/TypeDefs.hpp" 10 | #include "free_gait_core/leg_motion/Footstep.hpp" 11 | 12 | // gtest 13 | #include 14 | 15 | using namespace free_gait; 16 | 17 | TEST(footstep, triangleLowLongStep) 18 | { 19 | Footstep footstep(LimbEnum::LF_LEG); 20 | Position start(0.0, 0.0, 0.0); 21 | Position target(0.3, 0.0, 0.0); 22 | footstep.updateStartPosition(start); 23 | footstep.setTargetPosition("map", target); 24 | double height = 0.05; 25 | footstep.setProfileHeight(0.05); 26 | footstep.setProfileType("triangle"); 27 | footstep.setAverageVelocity(0.15); 28 | ASSERT_TRUE(footstep.compute(true)); 29 | 30 | for (double time = 0.0; time < footstep.getDuration(); time += 0.001) { 31 | Position position = footstep.evaluatePosition(time); 32 | EXPECT_LT(position.z(), height + 0.001); 33 | EXPECT_GT(position.x(), start.x() - 0.001); 34 | EXPECT_LT(position.x(), target.x() + 0.001); 35 | } 36 | } 37 | 38 | TEST(footstep, trianglehighLong) 39 | { 40 | Footstep footstep(LimbEnum::LF_LEG); 41 | Position start(0.0, 0.0, 0.0); 42 | Position target(0.3, 0.0, 0.3); 43 | footstep.updateStartPosition(start); 44 | footstep.setTargetPosition("map", target); 45 | double height = 0.05; 46 | footstep.setProfileHeight(0.05); 47 | footstep.setProfileType("triangle"); 48 | footstep.setAverageVelocity(0.15); 49 | ASSERT_TRUE(footstep.compute(true)); 50 | 51 | for (double time = 0.0; time < footstep.getDuration(); time += 0.001) { 52 | Position position = footstep.evaluatePosition(time); 53 | EXPECT_LT(position.z(), target.z() + height + 0.01); 54 | EXPECT_GT(position.x(), start.x() - 0.001); 55 | EXPECT_LT(position.x(), target.x() + 0.001); 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /qp_solver/test/StepTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * StepTest.cpp 3 | * 4 | * Created on: Oct 27, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #include "free_gait_core/TypeDefs.hpp" 10 | #include "free_gait_core/step/Step.hpp" 11 | 12 | // gtest 13 | #include 14 | 15 | using namespace free_gait; 16 | 17 | TEST(step, initialization) 18 | { 19 | Step step; 20 | ASSERT_FALSE(step.hasBaseMotion()); 21 | ASSERT_FALSE(step.hasLegMotion()); 22 | ASSERT_FALSE(step.hasLegMotion(LimbEnum::RF_LEG)); 23 | Step stepCopy(step); 24 | ASSERT_FALSE(step.hasBaseMotion()); 25 | ASSERT_FALSE(step.hasLegMotion()); 26 | ASSERT_FALSE(step.hasLegMotion(LimbEnum::RF_LEG)); 27 | } 28 | -------------------------------------------------------------------------------- /qp_solver/test/test_free_gait_core.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Created on: Jun 1, 2015 3 | * Author: Péter Fankhauser 4 | * Institute: ETH Zurich, Autonomous Systems Lab 5 | */ 6 | 7 | // gtest 8 | #include 9 | 10 | // Run all the tests that were declared with TEST() 11 | int main(int argc, char **argv) 12 | { 13 | testing::InitGoogleTest(&argc, argv); 14 | srand((int)time(0)); 15 | return RUN_ALL_TESTS(); 16 | } 17 | -------------------------------------------------------------------------------- /quadruped_model/include/quadruped_model/common/typedefs.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "kindr/Core" 4 | 5 | namespace romo 6 | { 7 | //private: 8 | // /* data */ 9 | //public: 10 | // romo(/* args */); 11 | // ~romo(); 12 | 13 | typedef kindr::HomTransformQuatD Pose; 14 | // typedef kindr::TwistGlobalD Twist; 15 | typedef kindr::TwistLinearVelocityLocalAngularVelocityD Twist; 16 | typedef kindr::RotationQuaternionD RotationQuaternion; 17 | typedef kindr::AngleAxisD AngleAxis; 18 | typedef kindr::RotationMatrixD RotationMatrix; 19 | typedef kindr::EulerAnglesZyxD EulerAnglesZyx; 20 | typedef kindr::EulerAnglesZyxDiffD EulerAnglesZyxDiff; 21 | typedef kindr::RotationVectorD RotationVector; 22 | typedef kindr::EulerAnglesXyzD EulerAnglesXyz; 23 | typedef kindr::EulerAnglesXyzDiffD EulerAnglesXyzDiff; 24 | typedef kindr::Position3D Position; 25 | typedef kindr::LocalAngularVelocity LocalAngularVelocity; 26 | typedef kindr::Velocity3D LinearVelocity; 27 | typedef kindr::Acceleration3D LinearAcceleration; 28 | typedef kindr::AngularAcceleration3D AngularAcceleration; 29 | typedef kindr::Force3D Force; 30 | typedef kindr::Torque3D Torque; 31 | typedef kindr::VectorTypeless3D Vector; 32 | }; 33 | 34 | //romo::romo(/* args */) 35 | //{ 36 | //} 37 | 38 | //romo::~romo() 39 | //{ 40 | //} 41 | -------------------------------------------------------------------------------- /quadruped_model/src/QuadrupedModel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * QuadrupedModel.cpp 3 | * Descriotion: 4 | * 5 | * Created on: Mar 3, 2019 6 | * Author: Shunyao Wang 7 | * Institute: Harbin Institute of Technology, Shenzhen 8 | */ 9 | #include "quadruped_model/QuadrupedModel.hpp" 10 | namespace quadruped_model { 11 | QuadrupedModel::QuadrupedModel() 12 | { 13 | 14 | } 15 | QuadrupedModel::~QuadrupedModel() 16 | { 17 | 18 | } 19 | }; //namespace 20 | -------------------------------------------------------------------------------- /quadruped_model/src/kinematicsTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * kinematicsTest.cpp 3 | * Descriotion: 4 | * 5 | * Created on: date, 2019 6 | * Author: Shunyao Wang 7 | * Institute: Harbin Institute of Technology, Shenzhen 8 | */ 9 | #include 10 | #include "quadruped_model/quadrupedkinematics.h" 11 | //#include "geometry_msgs/Twist.h" 12 | //#include "gazebo_msgs/ModelStates.h" 13 | #include "rbdl/Model.h" 14 | #include "rbdl/addons/urdfreader/urdfreader.h" 15 | 16 | using namespace std; 17 | using namespace quadruped_model; 18 | 19 | int main(int argc, char **argv) 20 | { 21 | ros::init(argc, argv, "kinematicsTest"); 22 | ros::NodeHandle nh; 23 | Eigen::MatrixXd jacobian; 24 | QuadrupedKinematics QK; 25 | // QK.LoadRobotDescriptionFromFile("/home/hitstar/catkin_ws/src/quadruped_locomotion-dev/quadruped_model/urdf/simpledog.urdf"); 26 | JointPositionsLimb joints(0,0,0); 27 | Pose result; 28 | HipPoseInBase test_enum; 29 | test_enum[LimbEnum::RF_LEG] = Pose(Position(0,0,0), RotationQuaternion()); 30 | if(QK.FowardKinematicsSolve(joints, LimbEnum::RF_LEG, result)) 31 | { 32 | EulerAnglesZyx eular_zyx(result.getRotation()); 33 | cout<<"Kinematics solve result:"< 2 | 3 | 4 | 5 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /rqt_free_gait_action/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rqt_free_gait_action 4 | 0.3.0 5 | A RQT plugin to select and execute Free Gait actions. 6 | Samuel Bachmann 7 | Samuel Bachmann 8 | BSD 9 | https://github.com/leggedrobotics/free_gait 10 | https://github.com/leggedrobotics/free_gait/issues 11 | 12 | catkin 13 | 14 | free_gait_msgs 15 | roscpp 16 | rqt_gui 17 | rqt_gui_cpp 18 | std_srvs 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /rqt_free_gait_action/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A GUI plugin to select and send free gait actions. 7 | 8 | 9 | 10 | 11 | folder 12 | Plugins related to... 13 | 14 | 15 | applications-graphics 16 | A GUI plugin to select and send free gait actions. 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /rqt_free_gait_action/resource/resources.qrc: -------------------------------------------------------------------------------- 1 | 2 | 3 | icons/16x16/refresh.svg 4 | 5 | -------------------------------------------------------------------------------- /rqt_free_gait_action/scripts/rqt_free_gait_action: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | 5 | from rqt_gui.main import Main 6 | 7 | main = Main() 8 | sys.exit(main.main(sys.argv, standalone='rqt_free_gait/FreeGaitActionPlugin')) 9 | -------------------------------------------------------------------------------- /rqt_free_gait_action/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['rqt_free_gait_action'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rqt_free_gait_monitor 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | 8 | 0.1.1 (2017-01-31) 9 | ------------------ 10 | * Initial release. 11 | * Contributors: Samuel Bachmann 12 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/launch/rqt_free_gait_monitor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rqt_free_gait_monitor 4 | 0.3.0 5 | A RQT plugin to monitor, pause, and stop Free Gait actions. 6 | Samuel Bachmann 7 | Samuel Bachmann 8 | BSD 9 | https://github.com/leggedrobotics/free_gait 10 | https://github.com/leggedrobotics/free_gait/issues 11 | 12 | catkin 13 | 14 | rqt_gui 15 | rqt_gui_cpp 16 | roscpp 17 | rospy 18 | std_msgs 19 | std_srvs 20 | free_gait_msgs 21 | boost 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A GUI plugin to monitor free gait. 5 | 6 | 7 | 8 | 9 | folder 10 | Plugins related to... 11 | 12 | 13 | applications-graphics 14 | A GUI plugin to monitor free gait. 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/resource/icons/16x16/go-down.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/resource/icons/16x16/go-up.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/resource/icons/16x16/pause.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/resource/icons/16x16/play.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/resource/icons/16x16/stop.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | image/svg+xml 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/resource/icons/16x16/warning.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/resource/icons/22x22/done.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/resource/icons/22x22/pause.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/resource/icons/22x22/play.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/resource/icons/22x22/stop.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | image/svg+xml 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/resource/resources.qrc: -------------------------------------------------------------------------------- 1 | 2 | 3 | icons/16x16/done.svg 4 | icons/16x16/failed.svg 5 | icons/16x16/go-bottom.svg 6 | icons/16x16/go-down.svg 7 | icons/16x16/go-top.svg 8 | icons/16x16/go-up.svg 9 | icons/16x16/pause.svg 10 | icons/16x16/play.svg 11 | icons/16x16/stop.svg 12 | icons/16x16/unknown.svg 13 | icons/16x16/warning.svg 14 | icons/16x16/expand.svg 15 | icons/16x16/collapse.svg 16 | icons/16x16/delete.svg 17 | 18 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/scripts/rqt_free_gait_monitor: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | 5 | from rqt_gui.main import Main 6 | 7 | main = Main() 8 | sys.exit(main.main(sys.argv, standalone='rqt_free_gait_monitor/FreeGaitMonitorPlugin')) 9 | -------------------------------------------------------------------------------- /rqt_free_gait_monitor/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['rqt_free_gait_monitor'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /single_leg_test/DataFloder/Forward_Dynamics.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/single_leg_test/DataFloder/Forward_Dynamics.mp4 -------------------------------------------------------------------------------- /single_leg_test/DataFloder/Forward_Dynamics_realsensordata.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/single_leg_test/DataFloder/Forward_Dynamics_realsensordata.mp4 -------------------------------------------------------------------------------- /single_leg_test/DataFloder/rf_leg_fixed_base_real_sensor.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/single_leg_test/DataFloder/rf_leg_fixed_base_real_sensor.mp4 -------------------------------------------------------------------------------- /single_leg_test/DataFloder/rf_leg_fixed_base_whole_process.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunyaoWang/quadruped_locomotion/fddfafba2c217ad0412d7045d1bb579ce034744b/single_leg_test/DataFloder/rf_leg_fixed_base_whole_process.mp4 -------------------------------------------------------------------------------- /single_leg_test/include/single_leg_test/algorithm.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ALGORITHM_HPP 2 | #define ALGORITHM_HPP 3 | 4 | #endif // ALGORITHM_HPP 5 | -------------------------------------------------------------------------------- /single_leg_test/include/single_leg_test/webotsheader_dynamics.hpp: -------------------------------------------------------------------------------- 1 | #ifndef WEBOTSHEADER_DYNAMICS_HPP 2 | #define WEBOTSHEADER_DYNAMICS_HPP 3 | #include 4 | #include 5 | #include 6 | #include "single_leg_test/model_test_header.hpp" 7 | 8 | class webotsolver:public MyRobotSolver 9 | { 10 | public: 11 | webotsolver(); 12 | void moternameinitialization(string& Motorname); 13 | void 14 | 15 | private: 16 | 17 | }; 18 | 19 | #endif // WEBOTSHEADER_DYNAMICS_HPP 20 | -------------------------------------------------------------------------------- /single_leg_test/test/example.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * RBDL - Rigid Body Dynamics Library 3 | * Copyright (c) 2011-2016 Martin Felis 4 | * 5 | * Licensed under the zlib license. See LICENSE for more details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | using namespace RigidBodyDynamics; 13 | using namespace RigidBodyDynamics::Math; 14 | 15 | int main (int argc, char* argv[]) { 16 | rbdl_check_api_version (RBDL_API_VERSION); 17 | 18 | Model* model = NULL; 19 | 20 | unsigned int body_a_id, body_b_id, body_c_id; 21 | Body body_a, body_b, body_c; 22 | Joint joint_a, joint_b, joint_c; 23 | 24 | model = new Model(); 25 | 26 | model->gravity = Vector3d (0., -9.81, 0.); 27 | 28 | body_a = Body (1., Vector3d (0.5, 0., 0.0), Vector3d (1., 1., 1.)); 29 | joint_a = Joint( 30 | JointTypeRevolute, 31 | Vector3d (0., 0., 1.) 32 | ); 33 | 34 | body_a_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); 35 | 36 | body_b = Body (1., Vector3d (0., 0.5, 0.), Vector3d (1., 1., 1.)); 37 | joint_b = Joint ( 38 | JointTypeRevolute, 39 | Vector3d (0., 0., 1.) 40 | ); 41 | 42 | body_b_id = model->AddBody(body_a_id, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); 43 | 44 | body_c = Body (0., Vector3d (0.5, 0., 0.), Vector3d (1., 1., 1.)); 45 | joint_c = Joint ( 46 | JointTypeRevolute, 47 | Vector3d (0., 0., 1.) 48 | ); 49 | 50 | body_c_id = model->AddBody(body_b_id, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); 51 | 52 | VectorNd Q = VectorNd::Zero (model->dof_count); 53 | VectorNd QDot = VectorNd::Zero (model->dof_count); 54 | VectorNd Tau = VectorNd::Zero (model->dof_count); 55 | VectorNd QDDot = VectorNd::Zero (model->dof_count); 56 | 57 | ForwardDynamics (*model, Q, QDot, Tau, QDDot); 58 | 59 | std::cout << QDDot.transpose() << std::endl; 60 | 61 | delete model; 62 | 63 | return 0; 64 | } 65 | 66 | -------------------------------------------------------------------------------- /single_leg_test/test/model_test.cpp: -------------------------------------------------------------------------------- 1 | #include "single_leg_test/model_test_header.hpp" 2 | #include 3 | #include 4 | 5 | using namespace RigidBodyDynamics; 6 | using namespace RigidBodyDynamics::Math; 7 | using namespace std; 8 | 9 | int main(int argc, char *argv[]) 10 | { 11 | rbdl_check_api_version(RBDL_API_VERSION); 12 | 13 | MyRobotSolver solver_test; 14 | solver_test.model_initialization(); 15 | solver_test.GetLengthofPlannedData(); 16 | solver_test.IDynamicsCalculation(); 17 | solver_test.FDynamicsCalculation(); 18 | return 0; 19 | } 20 | --------------------------------------------------------------------------------