URDF接口
/legged_common/scripts/generate_urdf.sh
bash
#!/usr/bin/env sh
mkdir -p /tmp/legged_control/
rosrun xacro xacro $1 robot_type:=$2 > /tmp/legged_control/$2.urdf
/legged_controllers/launch/load_controller.launch
xml
<param name="urdfFile" value="/tmp/legged_control/$(arg robot_type).urdf"/>
/legged_controllers/src/LeggedController.cpp
cpp
bool LeggedController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh)
{
// Initialize OCS2
std::string urdfFile;
controller_nh.getParam("/urdfFile", urdfFile);
setupLeggedInterface(taskFile, urdfFile, referenceFile, verbose);
// Visualization
// Hardware interface
}
void LeggedController::setupLeggedInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile,
bool verbose) {
leggedInterface_ = std::make_shared<LeggedInterface>(taskFile, urdfFile, referenceFile);
leggedInterface_->setupOptimalControlProblem(taskFile, urdfFile, referenceFile, verbose);
}
/legged_control/legged_examples/legged_unitree/legged_unitree_description/launch/empty_world.launch
xml
<param name="legged_robot_description" command="$(find xacro)/xacro $(find legged_unitree_description)/urdf/robot.xacro
robot_type:=$(arg robot_type)"/>
<node name="generate_urdf" pkg="legged_common" type="generate_urdf.sh" output="screen"
args="$(find legged_unitree_description)/urdf/robot.xacro $(arg robot_type)"/>
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" clear_params="true"
args="-z 0.5 -param legged_robot_description -urdf -model $(arg robot_type)" output="screen"/>
/legged_control/legged_examples/legged_unitree/legged_unitree_hw/launch/legged_unitree_hw.launch
xml
<launch>
<arg name="robot_type" default="$(env ROBOT_TYPE)" doc="Robot type: [a1, aliengo, go1, laikago]"/>
<param name="legged_robot_description" command="$(find xacro)/xacro $(find legged_unitree_description)/urdf/robot.xacro
robot_type:=$(arg robot_type)
"/>
<node name="generate_urdf" pkg="legged_common" type="generate_urdf.sh" output="screen"
args="$(find legged_unitree_description)/urdf/robot.xacro $(arg robot_type)"/>
<rosparam file="$(find legged_unitree_hw)/config/$(arg robot_type).yaml" command="load"/>
<param name="robot_type" value="$(arg robot_type)"/>
<include file="$(find legged_unitree_hw)/launch/$(arg robot_type).launch"/>
</launch>
/legged_control/legged_examples/legged_gazebo/src/LeggedHWSim.cpp
cpp
bool LeggedHWSim::initSim(const std::string& robot_namespace, ros::NodeHandle model_nh, gazebo::physics::ModelPtr parent_model,
const urdf::Model* urdf_model, std::vector<transmission_interface::TransmissionInfo> transmissions)
/home/bigdavid/legged_control/legged_hw
cpp
//Get params from param server and check whether these params are set. Load urdf of robot
// param root_nh Root node-handle of a ROS node.
// param robot_hw_nh Node-handle for robot hardware.
bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override;
// URDF model of the robot
std::shared_ptr<urdf::Model> urdfModel_;
/** \brief Load urdf of robot from param server.
*
* Load urdf of robot from param server.
*
* @param rootNh Root node-handle of a ROS node
* @return True if successful.
*/
bool loadUrdf(ros::NodeHandle& rootNh);
cpp
bool LeggedHW::loadUrdf(ros::NodeHandle& rootNh) {
std::string urdfString;
if (urdfModel_ == nullptr) {
urdfModel_ = std::make_shared<urdf::Model>();
}
// get the urdf param on param server
rootNh.getParam("legged_robot_description", urdfString);
return !urdfString.empty() && urdfModel_->initString(urdfString);
}
/home/bigdavid/legged_control/legged_hw/include/legged_hw/LeggedHWLoop.h
cpp
LeggedHWLoop(ros::NodeHandle& nh, std::shared_ptr<LeggedHW> hardware_interface);
// Abstract Hardware Interface for your robot
std::shared_ptr<LeggedHW> hardwareInterface_;
/home/bigdavid/legged_control/legged_interface/include/legged_interface/LeggedInterface.h
cpp
LeggedInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile,
bool useHardFrictionConeConstraint = false);
virtual void setupOptimalControlProblem(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile,
bool verbose);
/home/bigdavid/legged_control/legged_interface/src/LeggedInterface.cpp
cpp
// check that urdf file exists
// 检查URDF文件是否存在
boost::filesystem::path urdfFilePath(urdfFile);
if (boost::filesystem::exists(urdfFilePath)) {
std::cerr << "[LeggedInterface] Loading Pinocchio model from: " << urdfFilePath << std::endl;
} else {
throw std::invalid_argument("[LeggedInterface] URDF file not found: " + urdfFilePath.string());
}
void LeggedInterface::setupModel(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile,
bool /*verbose*/) {
// PinocchioInterface
pinocchioInterfacePtr_ =
std::make_unique<PinocchioInterface>(centroidal_model::createPinocchioInterface(urdfFile, modelSettings_.jointNames));
// CentroidalModelInfo
centroidalModelInfo_ = centroidal_model::createCentroidalModelInfo(
*pinocchioInterfacePtr_, centroidal_model::loadCentroidalType(taskFile),
centroidal_model::loadDefaultJointState(pinocchioInterfacePtr_->getModel().nq - 6, referenceFile), modelSettings_.contactNames3DoF,
modelSettings_.contactNames6DoF);
}
void LeggedInterface::setupReferenceManager(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile,
bool verbose) {
auto swingTrajectoryPlanner =
std::make_unique<SwingTrajectoryPlanner>(loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4);
referenceManagerPtr_ =
std::make_shared<SwitchedModelReferenceManager>(loadGaitSchedule(referenceFile, verbose), std::move(swingTrajectoryPlanner));
}