EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

state estimate tested

This commit is contained in:
Huang Zhenbiao 2024-09-25 21:01:50 +08:00
parent bfc5532ce3
commit 3149acf2ed
21 changed files with 2006 additions and 295 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
README.md
View File

@ -11,7 +11,7 @@ Todo List:
- [x] Unitree Guide Controller - [x] Unitree Guide Controller
- [x] Gazebo Simulation - [x] Gazebo Simulation
- [x] Leg PD Controller - [x] Leg PD Controller
- [ ] Contact Sensor - [x] Contact Sensor
- [ ] OCS2 Legged Control - [ ] OCS2 Legged Control

23
controllers/ocs2_quadruped_controller/CMakeLists.txt
View File

@ -17,6 +17,7 @@ set(CONTROLLER_INCLUDE_DEPENDS
ocs2_legged_robot_ros ocs2_legged_robot_ros
ocs2_self_collision ocs2_self_collision
control_input_msgs control_input_msgs
angles
nav_msgs nav_msgs
qpOASES qpOASES
) )
@ -55,12 +56,32 @@ target_include_directories(${PROJECT_NAME}
PUBLIC PUBLIC
${qpOASES_INCLUDE_DIR} ${qpOASES_INCLUDE_DIR}
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>") "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
PRIVATE
src)
ament_target_dependencies( ament_target_dependencies(
${PROJECT_NAME} PUBLIC ${PROJECT_NAME} PUBLIC
${CONTROLLER_INCLUDE_DEPENDS} ${CONTROLLER_INCLUDE_DEPENDS}
) )
pluginlib_export_plugin_description_file(controller_interface ocs2_quadruped_controller.xml)
install(
DIRECTORY include/
DESTINATION include/${PROJECT_NAME}
)
install(
TARGETS ${PROJECT_NAME}
EXPORT export_${PROJECT_NAME}
ARCHIVE DESTINATION lib/${PROJECT_NAME}
LIBRARY DESTINATION lib/${PROJECT_NAME}
RUNTIME DESTINATION bin
)
ament_export_dependencies(${CONTROLLER_INCLUDE_DEPENDS})
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
if (BUILD_TESTING) if (BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights # the following line skips the linter which checks for copyrights

65
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h Normal file
View File

@ -0,0 +1,65 @@
//
// Created by biao on 24-9-10.
//
#ifndef INTERFACE_H
#define INTERFACE_H
#include <vector>
#include <hardware_interface/loaned_command_interface.hpp>
#include <hardware_interface/loaned_state_interface.hpp>
#include <control_input_msgs/msg/inputs.hpp>
#include "ocs2_quadruped_controller/estimator/StateEstimateBase.h"
struct CtrlComponent {
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_torque_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_position_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_velocity_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_kp_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_kd_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_effort_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_position_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_velocity_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
imu_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
foot_force_state_interface_;
control_input_msgs::msg::Inputs control_inputs_;
int frequency_{};
std::shared_ptr<ocs2::legged_robot::StateEstimateBase> estimator_;
CtrlComponent() {
}
void clear() {
joint_torque_command_interface_.clear();
joint_position_command_interface_.clear();
joint_velocity_command_interface_.clear();
joint_kd_command_interface_.clear();
joint_kp_command_interface_.clear();
joint_effort_state_interface_.clear();
joint_position_state_interface_.clear();
joint_velocity_state_interface_.clear();
imu_state_interface_.clear();
foot_force_state_interface_.clear();
}
};
#endif //INTERFACE_H

11
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h
View File

@ -9,7 +9,6 @@
#include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h> #include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h> #include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
#include <realtime_tools/realtime_tools/realtime_buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs/tf2_geometry_msgs.hpp> #include <tf2_geometry_msgs/tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/transform_broadcaster.h> #include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h> #include <tf2_ros/transform_listener.h>
@ -19,6 +18,7 @@ namespace ocs2::legged_robot {
public: public:
KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &eeKinematics, const PinocchioEndEffectorKinematics &eeKinematics,
CtrlComponent &ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr &node); const rclcpp_lifecycle::LifecycleNode::SharedPtr &node);
vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) override; vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
@ -26,10 +26,6 @@ namespace ocs2::legged_robot {
void loadSettings(const std::string &taskFile, bool verbose); void loadSettings(const std::string &taskFile, bool verbose);
protected: protected:
void updateFromTopic();
void callback(const nav_msgs::msg::Odometry::SharedPtr &msg);
nav_msgs::msg::Odometry getOdomMsg(); nav_msgs::msg::Odometry getOdomMsg();
vector_t feetHeights_; vector_t feetHeights_;
@ -48,10 +44,5 @@ namespace ocs2::legged_robot {
matrix_t a_, b_, c_, q_, p_, r_; matrix_t a_, b_, c_, q_, p_, r_;
vector_t xHat_, ps_, vs_; vector_t xHat_, ps_, vs_;
// Topic
tf2::Transform world2odom_;
std::string frameOdom_, frameGuess_;
bool topicUpdated_;
}; };
} // namespace legged } // namespace legged

28
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h
View File

@ -16,6 +16,8 @@
#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h> #include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
#include <rclcpp_lifecycle/lifecycle_node.hpp> #include <rclcpp_lifecycle/lifecycle_node.hpp>
struct CtrlComponent;
namespace ocs2::legged_robot { namespace ocs2::legged_robot {
class StateEstimateBase { class StateEstimateBase {
public: public:
@ -23,27 +25,27 @@ namespace ocs2::legged_robot {
StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &eeKinematics, const PinocchioEndEffectorKinematics &eeKinematics,
CtrlComponent &ctrl_component,
rclcpp_lifecycle::LifecycleNode::SharedPtr node); rclcpp_lifecycle::LifecycleNode::SharedPtr node);
virtual void updateJointStates(const vector_t &jointPos, const vector_t &jointVel); virtual void updateJointStates();
virtual void updateContact(contact_flag_t contactFlag) { contactFlag_ = contactFlag; } virtual void updateContact();
virtual void updateImu(const Eigen::Quaternion<scalar_t> &quat, const vector3_t &angularVelLocal, virtual void updateImu();
const vector3_t &linearAccelLocal,
const matrix3_t &orientationCovariance, const matrix3_t &angularVelCovariance,
const matrix3_t &linearAccelCovariance);
virtual vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) = 0; virtual vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) = 0;
size_t getMode() { return stanceLeg2ModeNumber(contactFlag_); } size_t getMode() { return stanceLeg2ModeNumber(contact_flag_); }
protected: protected:
void updateAngular(const vector3_t &zyx, const vector_t &angularVel); void updateAngular(const vector3_t &zyx, const vector_t &angularVel);
void updateLinear(const vector_t &pos, const vector_t &linearVel); void updateLinear(const vector_t &pos, const vector_t &linearVel);
void publishMsgs(const nav_msgs::msg::Odometry &odom); void publishMsgs(const nav_msgs::msg::Odometry &odom) const;
CtrlComponent &ctrl_component_;
PinocchioInterface pinocchioInterface_; PinocchioInterface pinocchioInterface_;
CentroidalModelInfo info_; CentroidalModelInfo info_;
@ -51,15 +53,13 @@ namespace ocs2::legged_robot {
vector3_t zyxOffset_ = vector3_t::Zero(); vector3_t zyxOffset_ = vector3_t::Zero();
vector_t rbdState_; vector_t rbdState_;
contact_flag_t contactFlag_{}; contact_flag_t contact_flag_{};
Eigen::Quaternion<scalar_t> quat_; Eigen::Quaternion<scalar_t> quat_;
vector3_t angularVelLocal_, linearAccelLocal_; vector3_t angular_vel_local_, linear_accel_local_;
matrix3_t orientationCovariance_, angularVelCovariance_, linearAccelCovariance_; matrix3_t orientationCovariance_, angularVelCovariance_, linearAccelCovariance_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPub_; rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr posePub_; rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
rclcpp::Time lastPub_;
rclcpp_lifecycle::LifecycleNode::SharedPtr node_; rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
}; };

90
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h
View File

@ -27,43 +27,43 @@
namespace ocs2::legged_robot { namespace ocs2::legged_robot {
class LeggedInterface : public RobotInterface { class LeggedInterface : public RobotInterface {
public: public:
LeggedInterface(const std::string &taskFile, const std::string &urdfFile, const std::string &referenceFile, LeggedInterface(const std::string &task_file, const std::string &urdf_file, const std::string &reference_file,
bool useHardFrictionConeConstraint = false); bool use_hard_friction_cone_constraint = false);
~LeggedInterface() override = default; ~LeggedInterface() override = default;
virtual void setupOptimalControlProblem(const std::string &taskFile, const std::string &urdfFile, virtual void setupOptimalControlProblem(const std::string &task_file, const std::string &urdf_file,
const std::string &referenceFile, const std::string &reference_file,
bool verbose); bool verbose);
const OptimalControlProblem &getOptimalControlProblem() const override { return *problemPtr_; } const OptimalControlProblem &getOptimalControlProblem() const override { return *problem_ptr_; }
const ModelSettings &modelSettings() const { return modelSettings_; } const ModelSettings &modelSettings() const { return model_settings_; }
const ddp::Settings &ddpSettings() const { return ddpSettings_; } const ddp::Settings &ddpSettings() const { return ddp_settings_; }
const mpc::Settings &mpcSettings() const { return mpcSettings_; } const mpc::Settings &mpcSettings() const { return mpc_settings_; }
const rollout::Settings &rolloutSettings() const { return rolloutSettings_; } const rollout::Settings &rolloutSettings() const { return rollout_settings_; }
const sqp::Settings &sqpSettings() { return sqpSettings_; } const sqp::Settings &sqpSettings() { return sqp_settings_; }
const ipm::Settings &ipmSettings() { return ipmSettings_; } const ipm::Settings &ipmSettings() { return ipm_settings_; }
const vector_t &getInitialState() const { return initialState_; } const vector_t &getInitialState() const { return initial_state_; }
const RolloutBase &getRollout() const { return *rolloutPtr_; } const RolloutBase &getRollout() const { return *rollout_ptr_; }
PinocchioInterface &getPinocchioInterface() { return *pinocchioInterfacePtr_; } PinocchioInterface &getPinocchioInterface() { return *pinocchio_interface_ptr_; }
const CentroidalModelInfo &getCentroidalModelInfo() const { return centroidalModelInfo_; } const CentroidalModelInfo &getCentroidalModelInfo() const { return centroidal_model_info_; }
PinocchioGeometryInterface &getGeometryInterface() { return *geometryInterfacePtr_; } PinocchioGeometryInterface &getGeometryInterface() { return *geometry_interface_ptr_; }
std::shared_ptr<SwitchedModelReferenceManager> getSwitchedModelReferenceManagerPtr() const { std::shared_ptr<SwitchedModelReferenceManager> getSwitchedModelReferenceManagerPtr() const {
return referenceManagerPtr_; return reference_manager_ptr_;
} }
const Initializer &getInitializer() const override { return *initializerPtr_; } const Initializer &getInitializer() const override { return *initializer_ptr_; }
std::shared_ptr<ReferenceManagerInterface> getReferenceManagerPtr() const override { std::shared_ptr<ReferenceManagerInterface> getReferenceManagerPtr() const override {
return referenceManagerPtr_; return reference_manager_ptr_;
} }
protected: protected:
virtual void setupModel(const std::string &taskFile, const std::string &urdfFile, virtual void setupModel(const std::string &task_file, const std::string &urdf_file,
const std::string &referenceFile, bool verbose); const std::string &reference_file);
virtual void setupReferenceManager(const std::string &taskFile, const std::string &urdfFile, virtual void setupReferenceManager(const std::string &taskFile, const std::string &urdfFile,
const std::string &referenceFile, const std::string &referenceFile,
@ -76,7 +76,7 @@ namespace ocs2::legged_robot {
std::shared_ptr<GaitSchedule> loadGaitSchedule(const std::string &file, bool verbose) const; std::shared_ptr<GaitSchedule> loadGaitSchedule(const std::string &file, bool verbose) const;
std::unique_ptr<StateInputCost> getBaseTrackingCost(const std::string &taskFile, std::unique_ptr<StateInputCost> getBaseTrackingCost(const std::string &taskFile,
const CentroidalModelInfo &info, bool verbose); const CentroidalModelInfo &info, bool verbose);
matrix_t initializeInputCostWeight(const std::string &taskFile, const CentroidalModelInfo &info); matrix_t initializeInputCostWeight(const std::string &taskFile, const CentroidalModelInfo &info);
@ -84,43 +84,43 @@ namespace ocs2::legged_robot {
const std::string &taskFile, bool verbose); const std::string &taskFile, bool verbose);
std::unique_ptr<StateInputConstraint> getFrictionConeConstraint(size_t contactPointIndex, std::unique_ptr<StateInputConstraint> getFrictionConeConstraint(size_t contactPointIndex,
scalar_t frictionCoefficient); scalar_t frictionCoefficient);
std::unique_ptr<StateInputCost> getFrictionConeSoftConstraint(size_t contactPointIndex, std::unique_ptr<StateInputCost> getFrictionConeSoftConstraint(size_t contactPointIndex,
scalar_t frictionCoefficient, scalar_t frictionCoefficient,
const RelaxedBarrierPenalty::Config & const RelaxedBarrierPenalty::Config &
barrierPenaltyConfig); barrierPenaltyConfig);
std::unique_ptr<EndEffectorKinematics<scalar_t> > getEeKinematicsPtr(const std::vector<std::string> &footNames, std::unique_ptr<EndEffectorKinematics<scalar_t> > getEeKinematicsPtr(const std::vector<std::string> &foot_names,
const std::string &modelName); const std::string &model_name);
std::unique_ptr<StateInputConstraint> getZeroVelocityConstraint( std::unique_ptr<StateInputConstraint> getZeroVelocityConstraint(
const EndEffectorKinematics<scalar_t> &eeKinematics, const EndEffectorKinematics<scalar_t> &end_effector_kinematics,
size_t contactPointIndex); size_t contact_point_index);
std::unique_ptr<StateCost> getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface, std::unique_ptr<StateCost> getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface,
const std::string &taskFile, const std::string &taskFile,
const std::string &prefix, bool verbose); const std::string &prefix, bool verbose);
ModelSettings modelSettings_; ModelSettings model_settings_;
mpc::Settings mpcSettings_; mpc::Settings mpc_settings_;
ddp::Settings ddpSettings_; ddp::Settings ddp_settings_;
sqp::Settings sqpSettings_; sqp::Settings sqp_settings_;
ipm::Settings ipmSettings_; ipm::Settings ipm_settings_;
const bool useHardFrictionConeConstraint_; const bool use_hard_friction_cone_constraint_;
std::unique_ptr<PinocchioInterface> pinocchioInterfacePtr_; std::unique_ptr<PinocchioInterface> pinocchio_interface_ptr_;
CentroidalModelInfo centroidalModelInfo_; CentroidalModelInfo centroidal_model_info_;
std::unique_ptr<PinocchioGeometryInterface> geometryInterfacePtr_; std::unique_ptr<PinocchioGeometryInterface> geometry_interface_ptr_;
std::unique_ptr<OptimalControlProblem> problemPtr_; std::unique_ptr<OptimalControlProblem> problem_ptr_;
std::shared_ptr<SwitchedModelReferenceManager> referenceManagerPtr_; std::shared_ptr<SwitchedModelReferenceManager> reference_manager_ptr_;
rollout::Settings rolloutSettings_; rollout::Settings rollout_settings_;
std::unique_ptr<RolloutBase> rolloutPtr_; std::unique_ptr<RolloutBase> rollout_ptr_;
std::unique_ptr<Initializer> initializerPtr_; std::unique_ptr<Initializer> initializer_ptr_;
vector_t initialState_; vector_t initial_state_;
}; };
} // namespace legged } // namespace legged

9
controllers/ocs2_quadruped_controller/ocs2_quadruped_controller.xml
View File

@ -0,0 +1,9 @@
<library path="ocs2_quadruped_controller">
<class name="ocs2_quadruped_controller/Ocs2QuadrupedController"
type="ocs2::legged_robot::Ocs2QuadrupedController"
base_class_type="controller_interface::ControllerInterface">
<description>
Quadruped Controller based on OCS2 Legged Control.
</description>
</class>
</library>

1
controllers/ocs2_quadruped_controller/package.xml
View File

@ -15,6 +15,7 @@
<depend>control_input_msgs</depend> <depend>control_input_msgs</depend>
<depend>qpOASES</depend> <depend>qpOASES</depend>
<depend>ocs2_self_collision</depend> <depend>ocs2_self_collision</depend>
<depend>angles</depend>
<test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend> <test_depend>ament_lint_common</test_depend>

213
controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp
View File

@ -12,13 +12,116 @@
#include <ocs2_quadruped_controller/wbc/WeightedWbc.h> #include <ocs2_quadruped_controller/wbc/WeightedWbc.h>
#include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> #include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h>
#include <ocs2_sqp/SqpMpc.h> #include <ocs2_sqp/SqpMpc.h>
#include <angles/angles.h>
namespace ocs2::legged_robot { namespace ocs2::legged_robot {
using config_type = controller_interface::interface_configuration_type;
controller_interface::InterfaceConfiguration Ocs2QuadrupedController::command_interface_configuration() const {
controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
conf.names.reserve(joint_names_.size() * command_interface_types_.size());
for (const auto &joint_name: joint_names_) {
for (const auto &interface_type: command_interface_types_) {
conf.names.push_back(joint_name + "/" += interface_type);
}
}
return conf;
}
controller_interface::InterfaceConfiguration Ocs2QuadrupedController::state_interface_configuration() const {
controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
conf.names.reserve(joint_names_.size() * state_interface_types_.size());
for (const auto &joint_name: joint_names_) {
for (const auto &interface_type: state_interface_types_) {
conf.names.push_back(joint_name + "/" += interface_type);
}
}
for (const auto &interface_type: imu_interface_types_) {
conf.names.push_back(imu_name_ + "/" += interface_type);
}
for (const auto &interface_type: foot_force_interface_types_) {
conf.names.push_back(foot_force_name_ + "/" += interface_type);
}
return conf;
}
controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time &time,
const rclcpp::Duration &period) {
// State Estimate
const vector_t rbd_state = ctrl_comp_.estimator_->update(time, period);
currentObservation_.time += period.seconds();
const scalar_t yaw_last = currentObservation_.state(9);
currentObservation_.state = rbdConversions_->computeCentroidalStateFromRbdModel(rbd_state);
currentObservation_.state(9) = yaw_last + angles::shortest_angular_distance(yaw_last, currentObservation_.state(9));
currentObservation_.mode = stateEstimate_->getMode();
// Update the current state of the system
mpcMrtInterface_->setCurrentObservation(currentObservation_);
// Load the latest MPC policy
mpcMrtInterface_->updatePolicy();
// Evaluate the current policy
vector_t optimizedState, optimizedInput;
size_t plannedMode = 0; // The mode that is active at the time the policy is evaluated at.
mpcMrtInterface_->evaluatePolicy(currentObservation_.time, currentObservation_.state, optimizedState, optimizedInput, plannedMode);
// Whole body control
currentObservation_.input = optimizedInput;
wbcTimer_.startTimer();
vector_t x = wbc_->update(optimizedState, optimizedInput, rbd_state, plannedMode, period.seconds());
wbcTimer_.endTimer();
vector_t torque = x.tail(12);
vector_t posDes = centroidal_model::getJointAngles(optimizedState, legged_interface_->getCentroidalModelInfo());
vector_t velDes = centroidal_model::getJointVelocities(optimizedInput, legged_interface_->getCentroidalModelInfo());
// Safety check, if failed, stop the controller
if (!safetyChecker_->check(currentObservation_, optimizedState, optimizedInput)) {
RCLCPP_ERROR(get_node()->get_logger(), "[Legged Controller] Safety check failed, stopping the controller.");
}
for (int i = 0; i < joint_names_.size(); i++) {
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(torque(i));
ctrl_comp_.joint_position_command_interface_[i].get().set_value(posDes(i));
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(velDes(i));
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.0);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(3.0);
}
return controller_interface::return_type::OK;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_init() { controller_interface::CallbackReturn Ocs2QuadrupedController::on_init() {
// Initialize OCS2
urdf_file_ = auto_declare<std::string>("urdf_file", urdf_file_);
task_file_ = auto_declare<std::string>("task_file", task_file_);
reference_file_ = auto_declare<std::string>("reference_file", reference_file_);
verbose_ = false; verbose_ = false;
loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_); loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_);
setUpLeggedInterface(); // Hardware Parameters
joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_);
command_interface_types_ =
auto_declare<std::vector<std::string> >("command_interfaces", command_interface_types_);
state_interface_types_ =
auto_declare<std::vector<std::string> >("state_interfaces", state_interface_types_);
imu_name_ = auto_declare<std::string>("imu_name", imu_name_);
imu_interface_types_ = auto_declare<std::vector<std::string> >("imu_interfaces", state_interface_types_);
foot_force_name_ = auto_declare<std::string>("foot_force_name", foot_force_name_);
foot_force_interface_types_ =
auto_declare<std::vector<std::string> >("foot_force_interfaces", state_interface_types_);
setupLeggedInterface();
setupMpc(); setupMpc();
setupMrt(); setupMrt();
@ -47,7 +150,75 @@ namespace ocs2::legged_robot {
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
void Ocs2QuadrupedController::setUpLeggedInterface() { controller_interface::CallbackReturn Ocs2QuadrupedController::on_configure(
const rclcpp_lifecycle::State & /*previous_state*/) {
control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
"/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
// Handle message
ctrl_comp_.control_inputs_.command = msg->command;
ctrl_comp_.control_inputs_.lx = msg->lx;
ctrl_comp_.control_inputs_.ly = msg->ly;
ctrl_comp_.control_inputs_.rx = msg->rx;
ctrl_comp_.control_inputs_.ry = msg->ry;
});
get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/) {
// clear out vectors in case of restart
ctrl_comp_.clear();
// assign command interfaces
for (auto &interface: command_interfaces_) {
std::string interface_name = interface.get_interface_name();
if (const size_t pos = interface_name.find('/'); pos != std::string::npos) {
command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface);
} else {
command_interface_map_[interface_name]->push_back(interface);
}
}
// assign state interfaces
for (auto &interface: state_interfaces_) {
if (interface.get_prefix_name() == imu_name_) {
ctrl_comp_.imu_state_interface_.emplace_back(interface);
} else if (interface.get_prefix_name() == foot_force_name_) {
ctrl_comp_.foot_force_state_interface_.emplace_back(interface);
} else {
state_interface_map_[interface.get_interface_name()]->push_back(interface);
}
}
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_deactivate(
const rclcpp_lifecycle::State & /*previous_state*/) {
release_interfaces();
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_cleanup(
const rclcpp_lifecycle::State & /*previous_state*/) {
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_shutdown(
const rclcpp_lifecycle::State & /*previous_state*/) {
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_error(
const rclcpp_lifecycle::State & /*previous_state*/) {
return CallbackReturn::SUCCESS;
}
void Ocs2QuadrupedController::setupLeggedInterface() {
legged_interface_ = std::make_shared<LeggedInterface>(task_file_, urdf_file_, reference_file_); legged_interface_ = std::make_shared<LeggedInterface>(task_file_, urdf_file_, reference_file_);
legged_interface_->setupOptimalControlProblem(task_file_, urdf_file_, reference_file_, verbose_); legged_interface_->setupOptimalControlProblem(task_file_, urdf_file_, reference_file_, verbose_);
} }
@ -60,18 +231,21 @@ namespace ocs2::legged_robot {
legged_interface_->getCentroidalModelInfo()); legged_interface_->getCentroidalModelInfo());
const std::string robotName = "legged_robot"; const std::string robotName = "legged_robot";
// Todo Handle Gait Receive.
// Gait receiver // Gait receiver
auto gaitReceiverPtr = // auto gaitReceiverPtr =
std::make_shared<GaitReceiver>(this->get_node(), // std::make_shared<GaitReceiver>(this->get_node(),
legged_interface_->getSwitchedModelReferenceManagerPtr()-> // legged_interface_->getSwitchedModelReferenceManagerPtr()->
getGaitSchedule(), robotName); // getGaitSchedule(), robotName);
// ROS ReferenceManager // ROS ReferenceManager
auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>( // auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>(
robotName, legged_interface_->getReferenceManagerPtr()); // robotName, legged_interface_->getReferenceManagerPtr());
rosReferenceManagerPtr->subscribe(this->get_node()); // rosReferenceManagerPtr->subscribe(this->get_node());
mpc_->getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); // mpc_->getSolverPtr()->addSynchronizedModule(gaitReceiverPtr);
mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);
observationPublisher_ = nh.advertise<ocs2_msgs::msg::mpc_observation>(robotName + "_mpc_observation", 1); // observationPublisher_ = nh.advertise<ocs2_msgs::msg::mpc_observation>(robotName + "_mpc_observation", 1);
} }
void Ocs2QuadrupedController::setupMrt() { void Ocs2QuadrupedController::setupMrt() {
@ -80,11 +254,11 @@ namespace ocs2::legged_robot {
mpcTimer_.reset(); mpcTimer_.reset();
controllerRunning_ = true; controllerRunning_ = true;
mpcThread_ = std::thread([&]() { mpcThread_ = std::thread([&] {
while (controllerRunning_) { while (controllerRunning_) {
try { try {
executeAndSleep( executeAndSleep(
[&]() { [&] {
if (mpcRunning_) { if (mpcRunning_) {
mpcTimer_.startTimer(); mpcTimer_.startTimer();
mpcMrtInterface_->advanceMpc(); mpcMrtInterface_->advanceMpc();
@ -102,10 +276,13 @@ namespace ocs2::legged_robot {
} }
void Ocs2QuadrupedController::setupStateEstimate() { void Ocs2QuadrupedController::setupStateEstimate() {
stateEstimate_ = std::make_shared<KalmanFilterEstimate>(legged_interface_->getPinocchioInterface(), ctrl_comp_.estimator_ = std::make_shared<KalmanFilterEstimate>(legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo(), legged_interface_->getCentroidalModelInfo(),
*eeKinematicsPtr_, this->get_node()); *eeKinematicsPtr_, ctrl_comp_, this->get_node());
dynamic_cast<KalmanFilterEstimate &>(*stateEstimate_).loadSettings(task_file_, verbose_); dynamic_cast<KalmanFilterEstimate &>(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_);
currentObservation_.time = 0; currentObservation_.time = 0;
} }
} }
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(ocs2::legged_robot::Ocs2QuadrupedController, controller_interface::ControllerInterface);

43
controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h
View File

@ -4,13 +4,16 @@
#ifndef OCS2QUADRUPEDCONTROLLER_H #ifndef OCS2QUADRUPEDCONTROLLER_H
#define OCS2QUADRUPEDCONTROLLER_H #define OCS2QUADRUPEDCONTROLLER_H
#include "SafetyChecker.h"
#include <controller_interface/controller_interface.hpp> #include <controller_interface/controller_interface.hpp>
#include <ocs2_quadruped_controller/interface/LeggedInterface.h> #include <control_input_msgs/msg/inputs.hpp>
#include <ocs2_mpc/MPC_MRT_Interface.h>
#include <ocs2_centroidal_model/CentroidalModelRbdConversions.h> #include <ocs2_centroidal_model/CentroidalModelRbdConversions.h>
#include <ocs2_mpc/MPC_MRT_Interface.h>
#include <ocs2_quadruped_controller/estimator/StateEstimateBase.h> #include <ocs2_quadruped_controller/estimator/StateEstimateBase.h>
#include <ocs2_quadruped_controller/interface/LeggedInterface.h>
#include <ocs2_quadruped_controller/wbc/WbcBase.h> #include <ocs2_quadruped_controller/wbc/WbcBase.h>
#include "SafetyChecker.h"
#include "ocs2_quadruped_controller/control/CtrlComponent.h"
namespace ocs2::legged_robot { namespace ocs2::legged_robot {
class Ocs2QuadrupedController final : public controller_interface::ControllerInterface { class Ocs2QuadrupedController final : public controller_interface::ControllerInterface {
@ -56,7 +59,7 @@ namespace ocs2::legged_robot {
const rclcpp_lifecycle::State &previous_state) override; const rclcpp_lifecycle::State &previous_state) override;
protected: protected:
void setUpLeggedInterface(); void setupLeggedInterface();
void setupMpc(); void setupMpc();
@ -64,6 +67,38 @@ namespace ocs2::legged_robot {
void setupStateEstimate(); void setupStateEstimate();
CtrlComponent ctrl_comp_;
std::vector<std::string> joint_names_;
std::vector<std::string> command_interface_types_;
std::vector<std::string> state_interface_types_;
std::unordered_map<
std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *>
command_interface_map_ = {
{"effort", &ctrl_comp_.joint_torque_command_interface_},
{"position", &ctrl_comp_.joint_position_command_interface_},
{"velocity", &ctrl_comp_.joint_velocity_command_interface_},
{"kp", &ctrl_comp_.joint_kp_command_interface_},
{"kd", &ctrl_comp_.joint_kd_command_interface_}
};
std::unordered_map<
std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> > *>
state_interface_map_ = {
{"position", &ctrl_comp_.joint_position_state_interface_},
{"effort", &ctrl_comp_.joint_effort_state_interface_},
{"velocity", &ctrl_comp_.joint_velocity_state_interface_}
};
// IMU Sensor
std::string imu_name_;
std::vector<std::string> imu_interface_types_;
// Foot Force Sensor
std::string foot_force_name_;
std::vector<std::string> foot_force_interface_types_;
rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;
SystemObservation currentObservation_; SystemObservation currentObservation_;
std::string task_file_; std::string task_file_;

29
controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp
View File

@ -15,18 +15,20 @@
namespace ocs2::legged_robot { namespace ocs2::legged_robot {
KalmanFilterEstimate::KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, KalmanFilterEstimate::KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &eeKinematics, const PinocchioEndEffectorKinematics &eeKinematics,
CtrlComponent &ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr &node) const rclcpp_lifecycle::LifecycleNode::SharedPtr &node)
: StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics, std::move(node)), : StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics, ctrl_component,
node),
numContacts_(info_.numThreeDofContacts + info_.numSixDofContacts), numContacts_(info_.numThreeDofContacts + info_.numSixDofContacts),
dimContacts_(3 * numContacts_), dimContacts_(3 * numContacts_),
numState_(6 + dimContacts_), numState_(6 + dimContacts_),
numObserve_(2 * dimContacts_ + numContacts_), numObserve_(2 * dimContacts_ + numContacts_) {
topicUpdated_(false) {
xHat_.setZero(numState_); xHat_.setZero(numState_);
ps_.setZero(dimContacts_); ps_.setZero(dimContacts_);
vs_.setZero(dimContacts_); vs_.setZero(dimContacts_);
a_.setIdentity(numState_, numState_); a_.setIdentity(numState_, numState_);
b_.setZero(numState_, 3); b_.setZero(numState_, 3);
matrix_t c1(3, 6), c2(3, 6); matrix_t c1(3, 6), c2(3, 6);
c1 << matrix3_t::Identity(), matrix3_t::Zero(); c1 << matrix3_t::Identity(), matrix3_t::Zero();
c2 << matrix3_t::Zero(), matrix3_t::Identity(); c2 << matrix3_t::Zero(), matrix3_t::Identity();
@ -44,11 +46,13 @@ namespace ocs2::legged_robot {
feetHeights_.setZero(numContacts_); feetHeights_.setZero(numContacts_);
eeKinematics_->setPinocchioInterface(pinocchioInterface_); eeKinematics_->setPinocchioInterface(pinocchioInterface_);
world2odom_.setRotation(tf2::Quaternion::getIdentity());
} }
vector_t KalmanFilterEstimate::update(const rclcpp::Time &time, const rclcpp::Duration &period) { vector_t KalmanFilterEstimate::update(const rclcpp::Time &time, const rclcpp::Duration &period) {
updateJointStates();
updateContact();
updateImu();
scalar_t dt = period.seconds(); scalar_t dt = period.seconds();
a_.block(0, 3, 3, 3) = dt * matrix3_t::Identity(); a_.block(0, 3, 3, 3) = dt * matrix3_t::Identity();
b_.block(0, 0, 3, 3) = 0.5 * dt * dt * matrix3_t::Identity(); b_.block(0, 0, 3, 3) = 0.5 * dt * dt * matrix3_t::Identity();
@ -101,7 +105,7 @@ namespace ocs2::legged_robot {
int rIndex1 = i1; int rIndex1 = i1;
int rIndex2 = dimContacts_ + i1; int rIndex2 = dimContacts_ + i1;
int rIndex3 = 2 * dimContacts_ + i; int rIndex3 = 2 * dimContacts_ + i;
bool isContact = contactFlag_[i]; bool isContact = contact_flag_[i];
scalar_t high_suspect_number(100); scalar_t high_suspect_number(100);
q.block(qIndex, qIndex, 3, 3) = (isContact ? 1. : high_suspect_number) * q.block(qIndex, qIndex, 3, 3); q.block(qIndex, qIndex, 3, 3) = (isContact ? 1. : high_suspect_number) * q.block(qIndex, qIndex, 3, 3);
@ -115,7 +119,7 @@ namespace ocs2::legged_robot {
} }
vector3_t g(0, 0, -9.81); vector3_t g(0, 0, -9.81);
vector3_t accel = getRotationMatrixFromZyxEulerAngles(quatToZyx(quat_)) * linearAccelLocal_ + g; vector3_t accel = getRotationMatrixFromZyxEulerAngles(quatToZyx(quat_)) * linear_accel_local_ + g;
vector_t y(numObserve_); vector_t y(numObserve_);
y << ps_, vs_, feetHeights_; y << ps_, vs_, feetHeights_;
@ -142,11 +146,6 @@ namespace ocs2::legged_robot {
// p_.block(0, 0, 2, 2) /= 10.; // p_.block(0, 0, 2, 2) /= 10.;
// } // }
if (topicUpdated_) {
updateFromTopic();
topicUpdated_ = false;
}
updateLinear(xHat_.segment<3>(0), xHat_.segment<3>(3)); updateLinear(xHat_.segment<3>(0), xHat_.segment<3>(3));
auto odom = getOdomMsg(); auto odom = getOdomMsg();
@ -179,9 +178,9 @@ namespace ocs2::legged_robot {
odom.twist.twist.linear.x = twist.x(); odom.twist.twist.linear.x = twist.x();
odom.twist.twist.linear.y = twist.y(); odom.twist.twist.linear.y = twist.y();
odom.twist.twist.linear.z = twist.z(); odom.twist.twist.linear.z = twist.z();
odom.twist.twist.angular.x = angularVelLocal_.x(); odom.twist.twist.angular.x = angular_vel_local_.x();
odom.twist.twist.angular.y = angularVelLocal_.y(); odom.twist.twist.angular.y = angular_vel_local_.y();
odom.twist.twist.angular.z = angularVelLocal_.z(); odom.twist.twist.angular.z = angular_vel_local_.z();
for (int i = 0; i < 3; ++i) { for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) { for (int j = 0; j < 3; ++j) {
odom.twist.covariance[i * 6 + j] = p_.block<3, 3>(3, 3)(i, j); odom.twist.covariance[i * 6 + j] = p_.block<3, 3>(3, 3)(i, j);

89
controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp
View File

@ -9,39 +9,71 @@
#include <ocs2_robotic_tools/common/RotationDerivativesTransforms.h> #include <ocs2_robotic_tools/common/RotationDerivativesTransforms.h>
#include <memory> #include <memory>
#include "ocs2_quadruped_controller/control/CtrlComponent.h"
namespace ocs2::legged_robot { namespace ocs2::legged_robot {
using namespace legged_robot; using namespace legged_robot;
StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &eeKinematics, const PinocchioEndEffectorKinematics &eeKinematics,
CtrlComponent &ctrl_component,
rclcpp_lifecycle::LifecycleNode::SharedPtr node) rclcpp_lifecycle::LifecycleNode::SharedPtr node)
: pinocchioInterface_(std::move(pinocchioInterface)), : ctrl_component_(ctrl_component),
pinocchioInterface_(std::move(pinocchioInterface)),
info_(std::move(info)), info_(std::move(info)),
eeKinematics_(eeKinematics.clone()), eeKinematics_(eeKinematics.clone()),
rbdState_(vector_t::Zero(2 * info_.generalizedCoordinatesNum)), node_(std::move(node)) { rbdState_(vector_t::Zero(2 * info_.generalizedCoordinatesNum)), node_(std::move(node)) {
odomPub_ = node_->create_publisher<nav_msgs::msg::Odometry>("odom", 10); odom_pub_ = node_->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
posePub_ = node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("pose", 10); pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("pose", 10);
} }
void StateEstimateBase::updateJointStates(const vector_t &jointPos, const vector_t &jointVel) { void StateEstimateBase::updateJointStates() {
rbdState_.segment(6, info_.actuatedDofNum) = jointPos; const size_t size = ctrl_component_.joint_effort_state_interface_.size();
rbdState_.segment(6 + info_.generalizedCoordinatesNum, info_.actuatedDofNum) = jointVel; vector_t joint_pos(size), joint_vel(size);
for (int i = 0; i < size; i++) {
joint_pos(i) = ctrl_component_.joint_position_state_interface_[i].get().get_value();
joint_vel(i) = ctrl_component_.joint_velocity_state_interface_[i].get().get_value();
}
rbdState_.segment(6, info_.actuatedDofNum) = joint_pos;
rbdState_.segment(6 + info_.generalizedCoordinatesNum, info_.actuatedDofNum) = joint_vel;
} }
void StateEstimateBase::updateImu(const Eigen::Quaternion<scalar_t> &quat, const vector3_t &angularVelLocal, void StateEstimateBase::updateContact() {
const vector3_t &linearAccelLocal, const matrix3_t &orientationCovariance, const size_t size = ctrl_component_.foot_force_state_interface_.size();
const matrix3_t &angularVelCovariance, const matrix3_t &linearAccelCovariance) { for (int i = 0; i < size; i++) {
quat_ = quat; contact_flag_[i] = ctrl_component_.foot_force_state_interface_[i].get().get_value() > 1;
angularVelLocal_ = angularVelLocal; }
linearAccelLocal_ = linearAccelLocal; }
orientationCovariance_ = orientationCovariance;
angularVelCovariance_ = angularVelCovariance;
linearAccelCovariance_ = linearAccelCovariance;
vector3_t zyx = quatToZyx(quat) - zyxOffset_; void StateEstimateBase::updateImu() {
vector3_t angularVelGlobal = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives<scalar_t>( quat_ = {
zyx, getEulerAnglesZyxDerivativesFromLocalAngularVelocity<scalar_t>(quatToZyx(quat), angularVelLocal)); ctrl_component_.imu_state_interface_[0].get().get_value(),
ctrl_component_.imu_state_interface_[1].get().get_value(),
ctrl_component_.imu_state_interface_[2].get().get_value(),
ctrl_component_.imu_state_interface_[3].get().get_value()
};
angular_vel_local_ = {
ctrl_component_.imu_state_interface_[4].get().get_value(),
ctrl_component_.imu_state_interface_[5].get().get_value(),
ctrl_component_.imu_state_interface_[6].get().get_value()
};
linear_accel_local_ = {
ctrl_component_.imu_state_interface_[7].get().get_value(),
ctrl_component_.imu_state_interface_[8].get().get_value(),
ctrl_component_.imu_state_interface_[9].get().get_value()
};
// orientationCovariance_ = orientationCovariance;
// angularVelCovariance_ = angularVelCovariance;
// linearAccelCovariance_ = linearAccelCovariance;
const vector3_t zyx = quatToZyx(quat_) - zyxOffset_;
const vector3_t angularVelGlobal = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives<scalar_t>(
zyx, getEulerAnglesZyxDerivativesFromLocalAngularVelocity<scalar_t>(quatToZyx(quat_), angular_vel_local_));
updateAngular(zyx, angularVelGlobal); updateAngular(zyx, angularVelGlobal);
} }
@ -55,20 +87,13 @@ namespace ocs2::legged_robot {
rbdState_.segment<3>(info_.generalizedCoordinatesNum + 3) = linearVel; rbdState_.segment<3>(info_.generalizedCoordinatesNum + 3) = linearVel;
} }
void StateEstimateBase::publishMsgs(const nav_msgs::msg::Odometry &odom) { void StateEstimateBase::publishMsgs(const nav_msgs::msg::Odometry &odom) const {
rclcpp::Time time = odom.header.stamp; rclcpp::Time time = odom.header.stamp;
scalar_t publishRate = 200; odom_pub_->publish(odom);
// if (lastPub_ + rclcpp::Duration(1. / publishRate) < time) {
// lastPub_ = time; geometry_msgs::msg::PoseWithCovarianceStamped pose;
// if (odomPub_->trylock()) { pose.header = odom.header;
// odomPub_->msg_ = odom; pose.pose.pose = odom.pose.pose;
// odomPub_->unlockAndPublish(); pose_pub_->publish(pose);
// }
// if (posePub_->trylock()) {
// posePub_->msg_.header = odom.header;
// posePub_->msg_.pose = odom.pose;
// posePub_->unlockAndPublish();
// }
// }
} }
} // namespace legged } // namespace legged

276
controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp
View File

@ -2,6 +2,7 @@
// Created by qiayuan on 2022/7/16. // Created by qiayuan on 2022/7/16.
// //
#include <memory>
#include <pinocchio/fwd.hpp> // forward declarations must be included first. #include <pinocchio/fwd.hpp> // forward declarations must be included first.
#include <pinocchio/algorithm/frames.hpp> #include <pinocchio/algorithm/frames.hpp>
@ -35,166 +36,160 @@
#include <boost/filesystem/path.hpp> #include <boost/filesystem/path.hpp>
namespace ocs2::legged_robot { namespace ocs2::legged_robot {
LeggedInterface::LeggedInterface(const std::string &taskFile, const std::string &urdfFile, LeggedInterface::LeggedInterface(const std::string &task_file, const std::string &urdf_file,
const std::string &referenceFile, const std::string &reference_file,
bool useHardFrictionConeConstraint) const bool use_hard_friction_cone_constraint)
: useHardFrictionConeConstraint_(useHardFrictionConeConstraint) { : use_hard_friction_cone_constraint_(use_hard_friction_cone_constraint) {
// check that task file exists // check that task file exists
boost::filesystem::path taskFilePath(taskFile); if (const boost::filesystem::path task_file_path(task_file); exists(task_file_path)) {
if (boost::filesystem::exists(taskFilePath)) { std::cerr << "[LeggedInterface] Loading task file: " << task_file_path << std::endl;
std::cerr << "[LeggedInterface] Loading task file: " << taskFilePath << std::endl;
} else { } else {
throw std::invalid_argument("[LeggedInterface] Task file not found: " + taskFilePath.string()); throw std::invalid_argument("[LeggedInterface] Task file not found: " + task_file_path.string());
} }
// check that urdf file exists // check that urdf file exists
boost::filesystem::path urdfFilePath(urdfFile); if (const boost::filesystem::path urdf_file_path(urdf_file); exists(urdf_file_path)) {
if (boost::filesystem::exists(urdfFilePath)) { std::cerr << "[LeggedInterface] Loading Pinocchio model from: " << urdf_file_path << std::endl;
std::cerr << "[LeggedInterface] Loading Pinocchio model from: " << urdfFilePath << std::endl;
} else { } else {
throw std::invalid_argument("[LeggedInterface] URDF file not found: " + urdfFilePath.string()); throw std::invalid_argument("[LeggedInterface] URDF file not found: " + urdf_file_path.string());
} }
// check that targetCommand file exists // check that targetCommand file exists
boost::filesystem::path referenceFilePath(referenceFile); if (const boost::filesystem::path reference_file_path(reference_file); exists(reference_file_path)) {
if (boost::filesystem::exists(referenceFilePath)) { std::cerr << "[LeggedInterface] Loading target command settings from: " << reference_file_path << std::endl;
std::cerr << "[LeggedInterface] Loading target command settings from: " << referenceFilePath << std::endl;
} else { } else {
throw std::invalid_argument( throw std::invalid_argument(
"[LeggedInterface] targetCommand file not found: " + referenceFilePath.string()); "[LeggedInterface] targetCommand file not found: " + reference_file_path.string());
} }
bool verbose = false; bool verbose = false;
loadData::loadCppDataType(taskFile, "legged_robot_interface.verbose", verbose); loadData::loadCppDataType(task_file, "legged_robot_interface.verbose", verbose);
// load setting from loading file // load setting from loading file
modelSettings_ = loadModelSettings(taskFile, "model_settings", verbose); model_settings_ = loadModelSettings(task_file, "model_settings", verbose);
mpcSettings_ = mpc::loadSettings(taskFile, "mpc", verbose);
ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose); // Todo : load settings from ros param
sqpSettings_ = sqp::loadSettings(taskFile, "sqp", verbose); model_settings_.jointNames = {
ipmSettings_ = ipm::loadSettings(taskFile, "ipm", verbose); "FF_hip_joint", "FL_thigh_joint", "FL_calf_joint",
rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose); "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint",
"RL_hip_joint", "RL_thigh_joint", "RL_calf_joint",
"RR_hip_joint", "RR_thigh_joint", "RR_calf_joint"
};
model_settings_.contactNames3DoF = {"FL_foot", "FR_foot", "RL_foot", "RR_foot"};
mpc_settings_ = mpc::loadSettings(task_file, "mpc", verbose);
ddp_settings_ = ddp::loadSettings(task_file, "ddp", verbose);
sqp_settings_ = sqp::loadSettings(task_file, "sqp", verbose);
ipm_settings_ = ipm::loadSettings(task_file, "ipm", verbose);
rollout_settings_ = rollout::loadSettings(task_file, "rollout", verbose);
} }
/******************************************************************************************************/
/******************************************************************************************************/ void LeggedInterface::setupOptimalControlProblem(const std::string &task_file, const std::string &urdf_file,
/******************************************************************************************************/ const std::string &reference_file,
void LeggedInterface::setupOptimalControlProblem(const std::string &taskFile, const std::string &urdfFile, const bool verbose) {
const std::string &referenceFile, setupModel(task_file, urdf_file, reference_file);
bool verbose) {
setupModel(taskFile, urdfFile, referenceFile, verbose);
// Initial state // Initial state
initialState_.setZero(centroidalModelInfo_.stateDim); initial_state_.setZero(centroidal_model_info_.stateDim);
loadData::loadEigenMatrix(taskFile, "initialState", initialState_); loadData::loadEigenMatrix(task_file, "initialState", initial_state_);
setupReferenceManager(taskFile, urdfFile, referenceFile, verbose); setupReferenceManager(task_file, urdf_file, reference_file, verbose);
// Optimal control problem // Optimal control problem
problemPtr_ = std::make_unique<OptimalControlProblem>(); problem_ptr_ = std::make_unique<OptimalControlProblem>();
// Dynamics // Dynamics
std::unique_ptr<SystemDynamicsBase> dynamicsPtr; std::unique_ptr<SystemDynamicsBase> dynamicsPtr = std::make_unique<LeggedRobotDynamicsAD>(
dynamicsPtr = std::make_unique<LeggedRobotDynamicsAD>(*pinocchioInterfacePtr_, centroidalModelInfo_, "dynamics", *pinocchio_interface_ptr_, centroidal_model_info_, "dynamics",
modelSettings_); model_settings_);
problemPtr_->dynamicsPtr = std::move(dynamicsPtr); problem_ptr_->dynamicsPtr = std::move(dynamicsPtr);
// Cost terms // Cost terms
problemPtr_->costPtr->add("baseTrackingCost", getBaseTrackingCost(taskFile, centroidalModelInfo_, verbose)); problem_ptr_->costPtr->add("baseTrackingCost", getBaseTrackingCost(task_file, centroidal_model_info_, verbose));
// Constraint terms // Constraint terms
// friction cone settings // friction cone settings
scalar_t frictionCoefficient = 0.7; scalar_t frictionCoefficient = 0.7;
RelaxedBarrierPenalty::Config barrierPenaltyConfig; RelaxedBarrierPenalty::Config barrierPenaltyConfig;
std::tie(frictionCoefficient, barrierPenaltyConfig) = loadFrictionConeSettings(taskFile, verbose); std::tie(frictionCoefficient, barrierPenaltyConfig) = loadFrictionConeSettings(task_file, verbose);
for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { for (size_t i = 0; i < centroidal_model_info_.numThreeDofContacts; i++) {
const std::string &footName = modelSettings_.contactNames3DoF[i]; const std::string &footName = model_settings_.contactNames3DoF[i];
std::unique_ptr<EndEffectorKinematics<scalar_t> > eeKinematicsPtr = std::unique_ptr<EndEffectorKinematics<scalar_t> > eeKinematicsPtr =
getEeKinematicsPtr({footName}, footName); getEeKinematicsPtr({footName}, footName);
if (useHardFrictionConeConstraint_) { if (use_hard_friction_cone_constraint_) {
problemPtr_->inequalityConstraintPtr->add(footName + "_frictionCone", problem_ptr_->inequalityConstraintPtr->add(footName + "_frictionCone",
getFrictionConeConstraint(i, frictionCoefficient)); getFrictionConeConstraint(i, frictionCoefficient));
} else { } else {
problemPtr_->softConstraintPtr->add(footName + "_frictionCone", problem_ptr_->softConstraintPtr->add(footName + "_frictionCone",
getFrictionConeSoftConstraint( getFrictionConeSoftConstraint(
i, frictionCoefficient, barrierPenaltyConfig)); i, frictionCoefficient, barrierPenaltyConfig));
} }
problemPtr_->equalityConstraintPtr->add(footName + "_zeroForce", std::unique_ptr<StateInputConstraint>( problem_ptr_->equalityConstraintPtr->add(footName + "_zeroForce", std::make_unique<ZeroForceConstraint>(
new ZeroForceConstraint( *reference_manager_ptr_, i, centroidal_model_info_));
*referenceManagerPtr_, i, centroidalModelInfo_))); problem_ptr_->equalityConstraintPtr->add(footName + "_zeroVelocity",
problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity", getZeroVelocityConstraint(*eeKinematicsPtr, i));
getZeroVelocityConstraint(*eeKinematicsPtr, i)); problem_ptr_->equalityConstraintPtr->add(
problemPtr_->equalityConstraintPtr->add(
footName + "_normalVelocity", footName + "_normalVelocity",
std::unique_ptr<StateInputConstraint>( std::make_unique<NormalVelocityConstraintCppAd>(*reference_manager_ptr_, *eeKinematicsPtr, i));
new NormalVelocityConstraintCppAd(*referenceManagerPtr_, *eeKinematicsPtr, i)));
} }
// Self-collision avoidance constraint // Self-collision avoidance constraint
problemPtr_->stateSoftConstraintPtr->add("selfCollision", problem_ptr_->stateSoftConstraintPtr->add("selfCollision",
getSelfCollisionConstraint( getSelfCollisionConstraint(
*pinocchioInterfacePtr_, taskFile, "selfCollision", verbose)); *pinocchio_interface_ptr_, task_file, "selfCollision", verbose));
setupPreComputation(taskFile, urdfFile, referenceFile, verbose); setupPreComputation(task_file, urdf_file, reference_file, verbose);
// Rollout // Rollout
rolloutPtr_ = std::make_unique<TimeTriggeredRollout>(*problemPtr_->dynamicsPtr, rolloutSettings_); rollout_ptr_ = std::make_unique<TimeTriggeredRollout>(*problem_ptr_->dynamicsPtr, rollout_settings_);
// Initialization // Initialization
constexpr bool extendNormalizedNomentum = true; constexpr bool extendNormalizedNomentum = true;
initializerPtr_ = std::make_unique<LeggedRobotInitializer>(centroidalModelInfo_, *referenceManagerPtr_, initializer_ptr_ = std::make_unique<LeggedRobotInitializer>(centroidal_model_info_, *reference_manager_ptr_,
extendNormalizedNomentum); extendNormalizedNomentum);
} }
/******************************************************************************************************/
/******************************************************************************************************/ void LeggedInterface::setupModel(const std::string &task_file, const std::string &urdf_file,
/******************************************************************************************************/ const std::string &reference_file) {
void LeggedInterface::setupModel(const std::string &taskFile, const std::string &urdfFile,
const std::string &referenceFile,
bool /*verbose*/) {
// PinocchioInterface // PinocchioInterface
pinocchioInterfacePtr_ = pinocchio_interface_ptr_ =
std::make_unique<PinocchioInterface>( std::make_unique<PinocchioInterface>(
centroidal_model::createPinocchioInterface(urdfFile, modelSettings_.jointNames)); centroidal_model::createPinocchioInterface(urdf_file, model_settings_.jointNames));
// CentroidalModelInfo // CentroidalModelInfo
centroidalModelInfo_ = centroidal_model::createCentroidalModelInfo( centroidal_model_info_ = centroidal_model::createCentroidalModelInfo(
*pinocchioInterfacePtr_, centroidal_model::loadCentroidalType(taskFile), *pinocchio_interface_ptr_, centroidal_model::loadCentroidalType(task_file),
centroidal_model::loadDefaultJointState(pinocchioInterfacePtr_->getModel().nq - 6, referenceFile), centroidal_model::loadDefaultJointState(pinocchio_interface_ptr_->getModel().nq - 6, reference_file),
modelSettings_.contactNames3DoF, model_settings_.contactNames3DoF,
modelSettings_.contactNames6DoF); model_settings_.contactNames6DoF);
} }
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
void LeggedInterface::setupReferenceManager(const std::string &taskFile, const std::string &urdfFile, void LeggedInterface::setupReferenceManager(const std::string &taskFile, const std::string &urdfFile,
const std::string &referenceFile, const std::string &referenceFile,
bool verbose) { bool verbose) {
auto swingTrajectoryPlanner = auto swingTrajectoryPlanner =
std::make_unique<SwingTrajectoryPlanner>( std::make_unique<SwingTrajectoryPlanner>(
loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4); loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4);
referenceManagerPtr_ = reference_manager_ptr_ =
std::make_shared<SwitchedModelReferenceManager>(loadGaitSchedule(referenceFile, verbose), std::make_shared<SwitchedModelReferenceManager>(loadGaitSchedule(referenceFile, verbose),
std::move(swingTrajectoryPlanner)); std::move(swingTrajectoryPlanner));
} }
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
void LeggedInterface::setupPreComputation(const std::string &taskFile, const std::string &urdfFile, void LeggedInterface::setupPreComputation(const std::string &taskFile, const std::string &urdfFile,
const std::string &referenceFile, const std::string &referenceFile,
bool verbose) { bool verbose) {
problemPtr_->preComputationPtr = std::make_unique<LeggedRobotPreComputation>( problem_ptr_->preComputationPtr = std::make_unique<LeggedRobotPreComputation>(
*pinocchioInterfacePtr_, centroidalModelInfo_, *referenceManagerPtr_->getSwingTrajectoryPlanner(), *pinocchio_interface_ptr_, centroidal_model_info_, *reference_manager_ptr_->getSwingTrajectoryPlanner(),
modelSettings_); model_settings_);
} }
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::shared_ptr<GaitSchedule> LeggedInterface::loadGaitSchedule(const std::string &file, bool verbose) const { std::shared_ptr<GaitSchedule> LeggedInterface::loadGaitSchedule(const std::string &file, bool verbose) const {
const auto initModeSchedule = loadModeSchedule(file, "initialModeSchedule", false); const auto initModeSchedule = loadModeSchedule(file, "initialModeSchedule", false);
const auto defaultModeSequenceTemplate = loadModeSequenceTemplate(file, "defaultModeSequenceTemplate", false); const auto defaultModeSequenceTemplate = loadModeSequenceTemplate(file, "defaultModeSequenceTemplate", false);
@ -221,26 +216,24 @@ namespace ocs2::legged_robot {
} }
return std::make_shared<GaitSchedule>(initModeSchedule, defaultModeSequenceTemplate, return std::make_shared<GaitSchedule>(initModeSchedule, defaultModeSequenceTemplate,
modelSettings_.phaseTransitionStanceTime); model_settings_.phaseTransitionStanceTime);
} }
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
matrix_t LeggedInterface::initializeInputCostWeight(const std::string &taskFile, const CentroidalModelInfo &info) { matrix_t LeggedInterface::initializeInputCostWeight(const std::string &taskFile, const CentroidalModelInfo &info) {
const size_t totalContactDim = 3 * info.numThreeDofContacts; const size_t totalContactDim = 3 * info.numThreeDofContacts;
const auto &model = pinocchioInterfacePtr_->getModel(); const auto &model = pinocchio_interface_ptr_->getModel();
auto &data = pinocchioInterfacePtr_->getData(); auto &data = pinocchio_interface_ptr_->getData();
const auto q = centroidal_model::getGeneralizedCoordinates(initialState_, centroidalModelInfo_); const auto q = centroidal_model::getGeneralizedCoordinates(initial_state_, centroidal_model_info_);
pinocchio::computeJointJacobians(model, data, q); computeJointJacobians(model, data, q);
pinocchio::updateFramePlacements(model, data); updateFramePlacements(model, data);
matrix_t base2feetJac(totalContactDim, info.actuatedDofNum); matrix_t base2feetJac(totalContactDim, info.actuatedDofNum);
for (size_t i = 0; i < info.numThreeDofContacts; i++) { for (size_t i = 0; i < info.numThreeDofContacts; i++) {
matrix_t jac = matrix_t::Zero(6, info.generalizedCoordinatesNum); matrix_t jac = matrix_t::Zero(6, info.generalizedCoordinatesNum);
pinocchio::getFrameJacobian(model, data, model.getBodyId(modelSettings_.contactNames3DoF[i]), getFrameJacobian(model, data, model.getBodyId(model_settings_.contactNames3DoF[i]),
pinocchio::LOCAL_WORLD_ALIGNED, jac); pinocchio::LOCAL_WORLD_ALIGNED, jac);
base2feetJac.block(3 * i, 0, 3, info.actuatedDofNum) = jac.block(0, 6, 3, info.actuatedDofNum); base2feetJac.block(3 * i, 0, 3, info.actuatedDofNum) = jac.block(0, 6, 3, info.actuatedDofNum);
} }
@ -255,9 +248,7 @@ namespace ocs2::legged_robot {
return r; return r;
} }
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::unique_ptr<StateInputCost> LeggedInterface::getBaseTrackingCost( std::unique_ptr<StateInputCost> LeggedInterface::getBaseTrackingCost(
const std::string &taskFile, const CentroidalModelInfo &info, const std::string &taskFile, const CentroidalModelInfo &info,
bool verbose) { bool verbose) {
@ -274,16 +265,14 @@ namespace ocs2::legged_robot {
} }
return std::make_unique<LeggedRobotStateInputQuadraticCost>(std::move(Q), std::move(R), info, return std::make_unique<LeggedRobotStateInputQuadraticCost>(std::move(Q), std::move(R), info,
*referenceManagerPtr_); *reference_manager_ptr_);
} }
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::pair<scalar_t, RelaxedBarrierPenalty::Config> LeggedInterface::loadFrictionConeSettings( std::pair<scalar_t, RelaxedBarrierPenalty::Config> LeggedInterface::loadFrictionConeSettings(
const std::string &taskFile, bool verbose) { const std::string &taskFile, bool verbose) {
boost::property_tree::ptree pt; boost::property_tree::ptree pt;
boost::property_tree::read_info(taskFile, pt); read_info(taskFile, pt);
const std::string prefix = "frictionConeSoftConstraint."; const std::string prefix = "frictionConeSoftConstraint.";
scalar_t frictionCoefficient = 1.0; scalar_t frictionCoefficient = 1.0;
@ -302,19 +291,16 @@ namespace ocs2::legged_robot {
return {frictionCoefficient, barrierPenaltyConfig}; return {frictionCoefficient, barrierPenaltyConfig};
} }
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::unique_ptr<StateInputConstraint> LeggedInterface::getFrictionConeConstraint( std::unique_ptr<StateInputConstraint> LeggedInterface::getFrictionConeConstraint(
size_t contactPointIndex, scalar_t frictionCoefficient) { size_t contactPointIndex, scalar_t frictionCoefficient) {
FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient); FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient);
return std::make_unique<FrictionConeConstraint>(*referenceManagerPtr_, frictionConeConConfig, contactPointIndex, return std::make_unique<FrictionConeConstraint>(*reference_manager_ptr_, frictionConeConConfig,
centroidalModelInfo_); contactPointIndex,
centroidal_model_info_);
} }
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::unique_ptr<StateInputCost> LeggedInterface::getFrictionConeSoftConstraint( std::unique_ptr<StateInputCost> LeggedInterface::getFrictionConeSoftConstraint(
size_t contactPointIndex, scalar_t frictionCoefficient, size_t contactPointIndex, scalar_t frictionCoefficient,
const RelaxedBarrierPenalty::Config &barrierPenaltyConfig) { const RelaxedBarrierPenalty::Config &barrierPenaltyConfig) {
@ -323,39 +309,35 @@ namespace ocs2::legged_robot {
std::make_unique<RelaxedBarrierPenalty>(barrierPenaltyConfig)); std::make_unique<RelaxedBarrierPenalty>(barrierPenaltyConfig));
} }
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::unique_ptr<EndEffectorKinematics<scalar_t> > LeggedInterface::getEeKinematicsPtr(
const std::vector<std::string> &footNames,
const std::string &modelName) {
std::unique_ptr<EndEffectorKinematics<scalar_t> > eeKinematicsPtr;
const auto infoCppAd = centroidalModelInfo_.toCppAd(); std::unique_ptr<EndEffectorKinematics<scalar_t> > LeggedInterface::getEeKinematicsPtr(
const std::vector<std::string> &foot_names,
const std::string &model_name) {
const auto infoCppAd = centroidal_model_info_.toCppAd();
const CentroidalModelPinocchioMappingCppAd pinocchioMappingCppAd(infoCppAd); const CentroidalModelPinocchioMappingCppAd pinocchioMappingCppAd(infoCppAd);
auto velocityUpdateCallback = [&infoCppAd](const ad_vector_t &state, auto velocityUpdateCallback = [&infoCppAd](const ad_vector_t &state,
PinocchioInterfaceCppAd &pinocchioInterfaceAd) { PinocchioInterfaceCppAd &pinocchioInterfaceAd) {
const ad_vector_t q = centroidal_model::getGeneralizedCoordinates(state, infoCppAd); const ad_vector_t q = centroidal_model::getGeneralizedCoordinates(state, infoCppAd);
updateCentroidalDynamics(pinocchioInterfaceAd, infoCppAd, q); updateCentroidalDynamics(pinocchioInterfaceAd, infoCppAd, q);
}; };
eeKinematicsPtr.reset(new PinocchioEndEffectorKinematicsCppAd(*pinocchioInterfacePtr_, pinocchioMappingCppAd, std::unique_ptr<EndEffectorKinematics<scalar_t> > end_effector_kinematics = std::make_unique<
footNames, PinocchioEndEffectorKinematicsCppAd>(
centroidalModelInfo_.stateDim, *pinocchio_interface_ptr_, pinocchioMappingCppAd,
centroidalModelInfo_.inputDim, foot_names,
velocityUpdateCallback, modelName, centroidal_model_info_.stateDim,
modelSettings_.modelFolderCppAd, centroidal_model_info_.inputDim,
modelSettings_.recompileLibrariesCppAd, velocityUpdateCallback, model_name,
modelSettings_.verboseCppAd)); model_settings_.modelFolderCppAd,
model_settings_.recompileLibrariesCppAd,
model_settings_.verboseCppAd);
return eeKinematicsPtr; return end_effector_kinematics;
} }
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::unique_ptr<StateInputConstraint> LeggedInterface::getZeroVelocityConstraint( std::unique_ptr<StateInputConstraint> LeggedInterface::getZeroVelocityConstraint(
const EndEffectorKinematics<scalar_t> &eeKinematics, const EndEffectorKinematics<scalar_t> &end_effector_kinematics,
size_t contactPointIndex) { const size_t contact_point_index) {
auto eeZeroVelConConfig = [](scalar_t positionErrorGain) { auto eeZeroVelConConfig = [](scalar_t positionErrorGain) {
EndEffectorLinearConstraint::Config config; EndEffectorLinearConstraint::Config config;
config.b.setZero(3); config.b.setZero(3);
@ -366,14 +348,12 @@ namespace ocs2::legged_robot {
} }
return config; return config;
}; };
return std::unique_ptr<StateInputConstraint>(new ZeroVelocityConstraintCppAd( return std::make_unique<ZeroVelocityConstraintCppAd>(
*referenceManagerPtr_, eeKinematics, contactPointIndex, *reference_manager_ptr_, end_effector_kinematics, contact_point_index,
eeZeroVelConConfig(modelSettings_.positionErrorGain))); eeZeroVelConConfig(model_settings_.positionErrorGain));
} }
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::unique_ptr<StateCost> LeggedInterface::getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface, std::unique_ptr<StateCost> LeggedInterface::getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface,
const std::string &taskFile, const std::string &taskFile,
const std::string &prefix, const std::string &prefix,
@ -385,7 +365,7 @@ namespace ocs2::legged_robot {
scalar_t minimumDistance = 0.0; scalar_t minimumDistance = 0.0;
boost::property_tree::ptree pt; boost::property_tree::ptree pt;
boost::property_tree::read_info(taskFile, pt); read_info(taskFile, pt);
if (verbose) { if (verbose) {
std::cerr << "\n #### SelfCollision Settings: "; std::cerr << "\n #### SelfCollision Settings: ";
std::cerr << "\n #### =============================================================================\n"; std::cerr << "\n #### =============================================================================\n";
@ -396,16 +376,16 @@ namespace ocs2::legged_robot {
loadData::loadStdVectorOfPair(taskFile, prefix + ".collisionObjectPairs", collisionObjectPairs, verbose); loadData::loadStdVectorOfPair(taskFile, prefix + ".collisionObjectPairs", collisionObjectPairs, verbose);
loadData::loadStdVectorOfPair(taskFile, prefix + ".collisionLinkPairs", collisionLinkPairs, verbose); loadData::loadStdVectorOfPair(taskFile, prefix + ".collisionLinkPairs", collisionLinkPairs, verbose);
geometryInterfacePtr_ = std::make_unique<PinocchioGeometryInterface>( geometry_interface_ptr_ = std::make_unique<PinocchioGeometryInterface>(
pinocchioInterface, collisionLinkPairs, collisionObjectPairs); pinocchioInterface, collisionLinkPairs, collisionObjectPairs);
if (verbose) { if (verbose) {
std::cerr << " #### =============================================================================\n"; std::cerr << " #### =============================================================================\n";
const size_t numCollisionPairs = geometryInterfacePtr_->getNumCollisionPairs(); const size_t numCollisionPairs = geometry_interface_ptr_->getNumCollisionPairs();
std::cerr << "SelfCollision: Testing for " << numCollisionPairs << " collision pairs\n"; std::cerr << "SelfCollision: Testing for " << numCollisionPairs << " collision pairs\n";
} }
std::unique_ptr<StateConstraint> constraint = std::make_unique<LeggedSelfCollisionConstraint>( std::unique_ptr<StateConstraint> constraint = std::make_unique<LeggedSelfCollisionConstraint>(
CentroidalModelPinocchioMapping(centroidalModelInfo_), *geometryInterfacePtr_, minimumDistance); CentroidalModelPinocchioMapping(centroidal_model_info_), *geometry_interface_ptr_, minimumDistance);
auto penalty = std::make_unique<RelaxedBarrierPenalty>(RelaxedBarrierPenalty::Config{mu, delta}); auto penalty = std::make_unique<RelaxedBarrierPenalty>(RelaxedBarrierPenalty::Config{mu, delta});

53
descriptions/go1_description/config/robot_control.yaml
View File

@ -13,12 +13,65 @@ controller_manager:
unitree_guide_controller: unitree_guide_controller:
type: unitree_guide_controller/UnitreeGuideController type: unitree_guide_controller/UnitreeGuideController
ocs2_quadruped_controller:
type: ocs2_quadruped_controller/Ocs2QuadrupedController
imu_sensor_broadcaster: imu_sensor_broadcaster:
ros__parameters: ros__parameters:
sensor_name: "imu_sensor" sensor_name: "imu_sensor"
frame_id: "imu_link" frame_id: "imu_link"
unitree_guide_controller: unitree_guide_controller:
ros__parameters:
update_rate: 500 # Hz
joints:
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
command_interfaces:
- effort
- position
- velocity
- kp
- kd
state_interfaces:
- effort
- position
- velocity
feet_names:
- FR_foot
- FL_foot
- RR_foot
- RL_foot
imu_name: "imu_sensor"
base_name: "base"
imu_interfaces:
- orientation.w
- orientation.x
- orientation.y
- orientation.z
- angular_velocity.x
- angular_velocity.y
- angular_velocity.z
- linear_acceleration.x
- linear_acceleration.y
- linear_acceleration.z
ocs2_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 500 # Hz update_rate: 500 # Hz
joints: joints:

2
descriptions/go2_description/CMakeLists.txt
View File

@ -4,7 +4,7 @@ project(go2_description)
find_package(ament_cmake REQUIRED) find_package(ament_cmake REQUIRED)
install( install(
DIRECTORY meshes xacro launch config DIRECTORY meshes xacro launch config urdf
DESTINATION share/${PROJECT_NAME}/ DESTINATION share/${PROJECT_NAME}/
) )

16
descriptions/go2_description/README.md
View File

@ -20,11 +20,17 @@ source ~/ros2_ws/install/setup.bash
ros2 launch go2_description visualize.launch.py ros2 launch go2_description visualize.launch.py
``` ```
## Launch Hardware Interface ## Launch ROS2 Control
```bash * Unitree Guide Controller
source ~/ros2_ws/install/setup.bash ```bash
ros2 launch go2_description hardware.launch.py source ~/ros2_ws/install/setup.bash
``` ros2 launch go2_description hardware.launch.py
```
* OCS2 Quadruped Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch go2_description ocs2_control.launch.py
```
## When used for isaac gym or other similiar engine ## When used for isaac gym or other similiar engine

46
descriptions/go2_description/config/ocs2/reference.info Normal file
View File

@ -0,0 +1,46 @@
targetDisplacementVelocity 0.5;
targetRotationVelocity 1.57;
comHeight 0.3
defaultJointState
{
(0,0) -0.20 ; FL_hip_joint
(1,0) 0.72 ; FL_thigh_joint
(2,0) -1.44 ; FL_calf_joint
(3,0) -0.20 ; RL_hip_joint
(4,0) 0.72 ; RL_thigh_joint
(5,0) -1.44 ; RL_calf_joint
(6,0) 0.20 ; FR_hip_joint
(7,0) 0.72 ; FR_thigh_joint
(8,0) -1.44 ; FR_calf_joint
(9,0) 0.20 ; RR_hip_joint
(10,0) 0.72 ; RR_thigh_joint
(11,0) -1.44 ; RR_calf_joint
}
initialModeSchedule
{
modeSequence
{
[0] STANCE
[1] STANCE
}
eventTimes
{
[0] 0.5
}
}
defaultModeSequenceTemplate
{
modeSequence
{
[0] STANCE
}
switchingTimes
{
[0] 0.0
[1] 1.0
}
}

319
descriptions/go2_description/config/ocs2/task.info Normal file
View File

@ -0,0 +1,319 @@
centroidalModelType 0 // 0: FullCentroidalDynamics, 1: Single Rigid Body Dynamics
legged_robot_interface
{
verbose false // show the loaded parameters
}
model_settings
{
positionErrorGain 0.0
phaseTransitionStanceTime 0.1
verboseCppAd true
recompileLibrariesCppAd false
modelFolderCppAd /tmp/ocs2_quadruped_controller/go2
}
swing_trajectory_config
{
liftOffVelocity 0.05
touchDownVelocity -0.1
swingHeight 0.08
swingTimeScale 0.15
}
; DDP settings
ddp
{
algorithm SLQ
nThreads 3
threadPriority 50
maxNumIterations 1
minRelCost 1e-1
constraintTolerance 5e-3
displayInfo false
displayShortSummary false
checkNumericalStability false
debugPrintRollout false
debugCaching false
AbsTolODE 1e-5
RelTolODE 1e-3
maxNumStepsPerSecond 10000
timeStep 0.015
backwardPassIntegratorType ODE45
constraintPenaltyInitialValue 20.0
constraintPenaltyIncreaseRate 2.0
preComputeRiccatiTerms true
useFeedbackPolicy false
strategy LINE_SEARCH
lineSearch
{
minStepLength 1e-2
maxStepLength 1.0
hessianCorrectionStrategy DIAGONAL_SHIFT
hessianCorrectionMultiple 1e-5
}
}
; Multiple_Shooting SQP settings
sqp
{
nThreads 3
dt 0.015
sqpIteration 1
deltaTol 1e-4
g_max 1e-2
g_min 1e-6
inequalityConstraintMu 0.1
inequalityConstraintDelta 5.0
projectStateInputEqualityConstraints true
printSolverStatistics true
printSolverStatus false
printLinesearch false
useFeedbackPolicy false
integratorType RK2
threadPriority 50
}
; Multiple_Shooting IPM settings
ipm
{
nThreads 3
dt 0.015
ipmIteration 1
deltaTol 1e-4
g_max 10.0
g_min 1e-6
computeLagrangeMultipliers true
printSolverStatistics true
printSolverStatus false
printLinesearch false
useFeedbackPolicy false
integratorType RK2
threadPriority 50
initialBarrierParameter 1e-4
targetBarrierParameter 1e-4
barrierLinearDecreaseFactor 0.2
barrierSuperlinearDecreasePower 1.5
barrierReductionCostTol 1e-3
barrierReductionConstraintTol 1e-3
fractionToBoundaryMargin 0.995
usePrimalStepSizeForDual false
initialSlackLowerBound 1e-4
initialDualLowerBound 1e-4
initialSlackMarginRate 1e-2
initialDualMarginRate 1e-2
}
; Rollout settings
rollout
{
AbsTolODE 1e-5
RelTolODE 1e-3
timeStep 0.015
integratorType ODE45
maxNumStepsPerSecond 10000
checkNumericalStability false
}
mpc
{
timeHorizon 1.0 ; [s]
solutionTimeWindow -1 ; maximum [s]
coldStart false
debugPrint false
mpcDesiredFrequency 100 ; [Hz]
mrtDesiredFrequency 1000 ; [Hz] Useless
}
initialState
{
;; Normalized Centroidal Momentum: [linear, angular] ;;
(0,0) 0.0 ; vcom_x
(1,0) 0.0 ; vcom_y
(2,0) 0.0 ; vcom_z
(3,0) 0.0 ; L_x / robotMass
(4,0) 0.0 ; L_y / robotMass
(5,0) 0.0 ; L_z / robotMass
;; Base Pose: [position, orientation] ;;
(6,0) 0.0 ; p_base_x
(7,0) 0.0 ; p_base_y
(8,0) 0.3 ; p_base_z
(9,0) 0.0 ; theta_base_z
(10,0) 0.0 ; theta_base_y
(11,0) 0.0 ; theta_base_x
;; Leg Joint Positions: [FL, RL, FR, RR] ;;
(12,0) -0.20 ; FL_hip_joint
(13,0) 0.72 ; FL_thigh_joint
(14,0) -1.44 ; FL_calf_joint
(15,0) -0.20 ; RL_hip_joint
(16,0) 0.72 ; RL_thigh_joint
(17,0) -1.44 ; RL_calf_joint
(18,0) 0.20 ; FR_hip_joint
(19,0) 0.72 ; FR_thigh_joint
(20,0) -1.44 ; FR_calf_joint
(21,0) 0.20 ; RR_hip_joint
(22,0) 0.72 ; RR_thigh_joint
(23,0) -1.44 ; RR_calf_joint
}
; standard state weight matrix
Q
{
scaling 1e+0
;; Normalized Centroidal Momentum: [linear, angular] ;;
(0,0) 15.0 ; vcom_x
(1,1) 15.0 ; vcom_y
(2,2) 100.0 ; vcom_z
(3,3) 10.0 ; L_x / robotMass
(4,4) 30.0 ; L_y / robotMass
(5,5) 30.0 ; L_z / robotMass
;; Base Pose: [position, orientation] ;;
(6,6) 1000.0 ; p_base_x
(7,7) 1000.0 ; p_base_y
(8,8) 1500.0 ; p_base_z
(9,9) 100.0 ; theta_base_z
(10,10) 300.0 ; theta_base_y
(11,11) 300.0 ; theta_base_x
;; Leg Joint Positions: [FL, RL, FR, RR] ;;
(12,12) 5.0 ; FL_hip_joint
(13,13) 5.0 ; FL_thigh_joint
(14,14) 2.5 ; FL_calf_joint
(15,15) 5.0 ; RL_hip_joint
(16,16) 5.0 ; RL_thigh_joint
(17,17) 2.5 ; RL_calf_joint
(18,18) 5.0 ; FR_hip_joint
(19,19) 5.0 ; FR_thigh_joint
(20,20) 2.5 ; FR_calf_joint
(21,21) 5.0 ; RR_hip_joint
(22,22) 5.0 ; RR_thigh_joint
(23,23) 2.5 ; RR_calf_joint
}
; control weight matrix
R
{
scaling 1e-3
;; Feet Contact Forces: [FL, FR, RL, RR] ;;
(0,0) 1.0 ; front_left_force
(1,1) 1.0 ; front_left_force
(2,2) 1.0 ; front_left_force
(3,3) 1.0 ; front_right_force
(4,4) 1.0 ; front_right_force
(5,5) 1.0 ; front_right_force
(6,6) 1.0 ; rear_left_force
(7,7) 1.0 ; rear_left_force
(8,8) 1.0 ; rear_left_force
(9,9) 1.0 ; rear_right_force
(10,10) 1.0 ; rear_right_force
(11,11) 1.0 ; rear_right_force
;; foot velocity relative to base: [FL, RL, FR, RR] (uses the Jacobian at nominal configuration) ;;
(12,12) 5000.0 ; x
(13,13) 5000.0 ; y
(14,14) 5000.0 ; z
(15,15) 5000.0 ; x
(16,16) 5000.0 ; y
(17,17) 5000.0 ; z
(18,18) 5000.0 ; x
(19,19) 5000.0 ; y
(20,20) 5000.0 ; z
(21,21) 5000.0 ; x
(22,22) 5000.0 ; y
(23,23) 5000.0 ; z
}
frictionConeSoftConstraint
{
frictionCoefficient 0.3
; relaxed log barrier parameters
mu 0.1
delta 5.0
}
selfCollision
{
; Self Collision raw object pairs
collisionObjectPairs
{
}
; Self Collision pairs
collisionLinkPairs
{
[0] "FL_calf, FR_calf"
[1] "RL_calf, RR_calf"
[2] "FL_calf, RL_calf"
[3] "FR_calf, RR_calf"
[4] "FL_foot, FR_foot"
[5] "RL_foot, RR_foot"
[6] "FL_foot, RL_foot"
[7] "FR_foot, RR_foot"
}
minimumDistance 0.05
; relaxed log barrier parameters
mu 1e-2
delta 1e-3
}
; Whole body control
torqueLimitsTask
{
(0,0) 33.5 ; HAA
(1,0) 33.5 ; HFE
(2,0) 33.5 ; KFE
}
frictionConeTask
{
frictionCoefficient 0.3
}
swingLegTask
{
kp 350
kd 37
}
weight
{
swingLeg 100
baseAccel 1
contactForce 0.01
}
; State Estimation
kalmanFilter
{
footRadius 0.02
imuProcessNoisePosition 0.02
imuProcessNoiseVelocity 0.02
footProcessNoisePosition 0.002
footSensorNoisePosition 0.005
footSensorNoiseVelocity 0.1
footHeightSensorNoise 0.01
}

68
descriptions/go2_description/config/robot_control.yaml
View File

@ -13,6 +13,9 @@ controller_manager:
unitree_guide_controller: unitree_guide_controller:
type: unitree_guide_controller/UnitreeGuideController type: unitree_guide_controller/UnitreeGuideController
ocs2_quadruped_controller:
type: ocs2_quadruped_controller/Ocs2QuadrupedController
imu_sensor_broadcaster: imu_sensor_broadcaster:
ros__parameters: ros__parameters:
sensor_name: "imu_sensor" sensor_name: "imu_sensor"
@ -66,4 +69,67 @@ unitree_guide_controller:
- angular_velocity.z - angular_velocity.z
- linear_acceleration.x - linear_acceleration.x
- linear_acceleration.y - linear_acceleration.y
- linear_acceleration.z - linear_acceleration.z
ocs2_quadruped_controller:
ros__parameters:
update_rate: 500 # Hz
# urdf_file: "$(find legged_controllers)/config/$(arg robot_type)/robot.urdf"
# task_file: "$(find go2_description)/config/ocs2/task.info"
# reference_file: "$(find go2_description)/config/ocs2/reference.info"
joints:
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
command_interfaces:
- effort
- position
- velocity
- kp
- kd
state_interfaces:
- effort
- position
- velocity
feet_names:
- FL_foot
- RL_foot
- FR_foot
- RR_foot
imu_name: "imu_sensor"
base_name: "base"
imu_interfaces:
- orientation.w
- orientation.x
- orientation.y
- orientation.z
- angular_velocity.x
- angular_velocity.y
- angular_velocity.z
- linear_acceleration.x
- linear_acceleration.y
- linear_acceleration.z
foot_force_name: "foot_force"
foot_force_interfaces:
- FL
- RL
- FR
- RR

120
descriptions/go2_description/launch/ocs2_control.launch.py Normal file
View File

@ -0,0 +1,120 @@
import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from sympy.physics.vector.printing import params
package_description = "go2_description"
def process_xacro(context):
robot_type_value = context.launch_configurations['robot_type']
pkg_path = os.path.join(get_package_share_directory(package_description))
xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
robot_description_config = xacro.process_file(xacro_file, mappings={'robot_type': robot_type_value})
return (robot_description_config.toxml(), robot_type_value)
def launch_setup(context, *args, **kwargs):
(robot_description, robot_type) = process_xacro(context)
robot_controllers = PathJoinSubstitution(
[
FindPackageShare(package_description),
"config",
"robot_control.yaml",
]
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[
{
'publish_frequency': 20.0,
'use_tf_static': True,
'robot_description': robot_description,
'ignore_timestamp': True
}
],
)
controller_manager = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_controllers,
{
'urdf_file': os.path.join(get_package_share_directory(package_description), 'urdf', 'robot.urdf'),
'task_file': os.path.join(get_package_share_directory(package_description), 'config', 'ocs2',
'task.info'),
'reference_file': os.path.join(get_package_share_directory(package_description), 'config',
'ocs2', 'reference.info')
}],
output="both",
)
joint_state_publisher = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
)
imu_sensor_broadcaster = Node(
package="controller_manager",
executable="spawner",
arguments=["imu_sensor_broadcaster",
"--controller-manager", "/controller_manager"],
)
unitree_guide_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"]
)
return [
robot_state_publisher,
controller_manager,
joint_state_publisher,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_publisher,
on_exit=[imu_sensor_broadcaster],
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=imu_sensor_broadcaster,
on_exit=[unitree_guide_controller],
)
),
]
def generate_launch_description():
robot_type_arg = DeclareLaunchArgument(
'robot_type',
default_value='go2',
description='Type of the robot'
)
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
return LaunchDescription([
robot_type_arg,
OpaqueFunction(function=launch_setup),
Node(
package='rviz2',
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
)
])

798
descriptions/go2_description/urdf/robot.urdf Normal file
View File

@ -0,0 +1,798 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from robot.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="go2">
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 0.0"/>
</material>
<material name="silver">
<color rgba="0.9137254901960784 0.9137254901960784 0.8470588235294118 1.0"/>
</material>
<material name="orange">
<color rgba="1.0 0.4235294117647059 0.0392156862745098 1.0"/>
</material>
<material name="brown">
<color rgba="0.8705882352941177 0.8117647058823529 0.7647058823529411 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<ros2_control name="UnitreeSystem" type="system">
<hardware>
<plugin>hardware_unitree_mujoco/HardwareUnitree</plugin>
</hardware>
<joint name="FR_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FR_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FR_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<sensor name="imu_sensor">
<state_interface name="orientation.w"/>
<state_interface name="orientation.x"/>
<state_interface name="orientation.y"/>
<state_interface name="orientation.z"/>
<state_interface name="angular_velocity.x"/>
<state_interface name="angular_velocity.y"/>
<state_interface name="angular_velocity.z"/>
<state_interface name="linear_acceleration.x"/>
<state_interface name="linear_acceleration.y"/>
<state_interface name="linear_acceleration.z"/>
</sensor>
<sensor name="foot_force">
<state_interface name="FR"/>
<state_interface name="FL"/>
<state_interface name="RR"/>
<state_interface name="RL"/>
</sensor>
</ros2_control>
<!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
<!-- Rotor related joint and link is only for demonstrate location. -->
<!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/trunk.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.3762 0.0935 0.114"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.021112 0.0 -0.005366"/>
<mass value="6.921"/>
<inertia ixx="0.02448" ixy="0.00012166" ixz="0.0014849" iyy="0.098077" iyz="-3.12e-05" izz="0.107"/>
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<!-- <material name="red"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
<joint name="FR_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.1934 -0.0465 0"/>
<parent link="trunk"/>
<child link="FR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.0" friction="0.0"/>
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
</joint>
<link name="FR_hip">
<visual>
<origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/hip.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 -0.0955 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.0054 -0.00194 -0.000105"/>
<mass value="0.678"/>
<inertia ixx="0.00048" ixy="3.01e-06" ixz="1.11e-06" iyy="0.000884" iyz="1.42e-06" izz="0.000596"/>
</inertial>
</link>
<joint name="FR_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.0955 0"/>
<parent link="FR_hip"/>
<child link="FR_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.0" friction="0.0"/>
<limit effort="23.7" lower="-1.5708" upper="3.4907" velocity="30.1"/>
</joint>
<link name="FR_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.00374 0.0223 -0.0327"/>
<mass value="1.152"/>
<inertia ixx="0.00584" ixy="-8.72e-05" ixz="-0.000289" iyy="0.0058" iyz="-0.000808" izz="0.00103"/>
</inertial>
</link>
<joint name="FR_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="FR_thigh"/>
<child link="FR_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.0" friction="0.0"/>
<limit effort="35.55" lower="-2.7227" upper="-0.83776" velocity="20.06"/>
</joint>
<link name="FR_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/calf.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.00548 -0.000975 -0.115"/>
<mass value="0.154"/>
<inertia ixx="0.00108" ixy="3.4e-07" ixz="1.72e-05" iyy="0.0011" iyz="8.28e-06" izz="3.29e-05"/>
</inertial>
</link>
<joint name="FR_foot_fixed" type="fixed">
0
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="FR_calf"/>
<child link="FR_foot"/>
</joint>
<link name="FR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
</inertial>
</link>
<transmission name="FR_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FR_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FR_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FR_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FR_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FR_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FR_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FR_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FR_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<joint name="FL_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.1934 0.0465 0"/>
<parent link="trunk"/>
<child link="FL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.0" friction="0.0"/>
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
</joint>
<link name="FL_hip">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/hip.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0.0955 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.0054 0.00194 -0.000105"/>
<mass value="0.678"/>
<inertia ixx="0.00048" ixy="-3.01e-06" ixz="1.11e-06" iyy="0.000884" iyz="-1.42e-06" izz="0.000596"/>
</inertial>
</link>
<joint name="FL_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0.0955 0"/>
<parent link="FL_hip"/>
<child link="FL_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.0" friction="0.0"/>
<limit effort="23.7" lower="-1.5708" upper="3.4907" velocity="30.1"/>
</joint>
<link name="FL_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/thigh.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.00374 -0.0223 -0.0327"/>
<mass value="1.152"/>
<inertia ixx="0.00584" ixy="8.72e-05" ixz="-0.000289" iyy="0.0058" iyz="0.000808" izz="0.00103"/>
</inertial>
</link>
<joint name="FL_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="FL_thigh"/>
<child link="FL_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.0" friction="0.0"/>
<limit effort="35.55" lower="-2.7227" upper="-0.83776" velocity="20.06"/>
</joint>
<link name="FL_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/calf.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.00548 -0.000975 -0.115"/>
<mass value="0.154"/>
<inertia ixx="0.00108" ixy="3.4e-07" ixz="1.72e-05" iyy="0.0011" iyz="8.28e-06" izz="3.29e-05"/>
</inertial>
</link>
<joint name="FL_foot_fixed" type="fixed">
0
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="FL_calf"/>
<child link="FL_foot"/>
</joint>
<link name="FL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
</inertial>
</link>
<transmission name="FL_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FL_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FL_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FL_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FL_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FL_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FL_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FL_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FL_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<joint name="RR_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.1934 -0.0465 0"/>
<parent link="trunk"/>
<child link="RR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.0" friction="0.0"/>
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
</joint>
<link name="RR_hip">
<visual>
<origin rpy="3.141592653589793 3.141592653589793 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/hip.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 -0.0955 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0054 -0.00194 -0.000105"/>
<mass value="0.678"/>
<inertia ixx="0.00048" ixy="-3.01e-06" ixz="-1.11e-06" iyy="0.000884" iyz="1.42e-06" izz="0.000596"/>
</inertial>
</link>
<joint name="RR_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.0955 0"/>
<parent link="RR_hip"/>
<child link="RR_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.0" friction="0.0"/>
<limit effort="23.7" lower="-1.5708" upper="3.4907" velocity="30.1"/>
</joint>
<link name="RR_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.00374 0.0223 -0.0327"/>
<mass value="1.152"/>
<inertia ixx="0.00584" ixy="-8.72e-05" ixz="-0.000289" iyy="0.0058" iyz="-0.000808" izz="0.00103"/>
</inertial>
</link>
<joint name="RR_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="RR_thigh"/>
<child link="RR_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.0" friction="0.0"/>
<limit effort="35.55" lower="-2.7227" upper="-0.83776" velocity="20.06"/>
</joint>
<link name="RR_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/calf.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.00548 -0.000975 -0.115"/>
<mass value="0.154"/>
<inertia ixx="0.00108" ixy="3.4e-07" ixz="1.72e-05" iyy="0.0011" iyz="8.28e-06" izz="3.29e-05"/>
</inertial>
</link>
<joint name="RR_foot_fixed" type="fixed">
0
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="RR_calf"/>
<child link="RR_foot"/>
</joint>
<link name="RR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
</inertial>
</link>
<transmission name="RR_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RR_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RR_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RR_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RR_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RR_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RR_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RR_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RR_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<joint name="RL_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.1934 0.0465 0"/>
<parent link="trunk"/>
<child link="RL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.0" friction="0.0"/>
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
</joint>
<link name="RL_hip">
<visual>
<origin rpy="0 3.141592653589793 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/hip.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0.0955 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0054 0.00194 -0.000105"/>
<mass value="0.678"/>
<inertia ixx="0.00048" ixy="3.01e-06" ixz="-1.11e-06" iyy="0.000884" iyz="-1.42e-06" izz="0.000596"/>
</inertial>
</link>
<joint name="RL_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0.0955 0"/>
<parent link="RL_hip"/>
<child link="RL_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.0" friction="0.0"/>
<limit effort="23.7" lower="-1.5708" upper="3.4907" velocity="30.1"/>
</joint>
<link name="RL_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/thigh.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.00374 -0.0223 -0.0327"/>
<mass value="1.152"/>
<inertia ixx="0.00584" ixy="8.72e-05" ixz="-0.000289" iyy="0.0058" iyz="0.000808" izz="0.00103"/>
</inertial>
</link>
<joint name="RL_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="RL_thigh"/>
<child link="RL_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.0" friction="0.0"/>
<limit effort="35.55" lower="-2.7227" upper="-0.83776" velocity="20.06"/>
</joint>
<link name="RL_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/calf.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.00548 -0.000975 -0.115"/>
<mass value="0.154"/>
<inertia ixx="0.00108" ixy="3.4e-07" ixz="1.72e-05" iyy="0.0011" iyz="8.28e-06" izz="3.29e-05"/>
</inertial>
</link>
<joint name="RL_foot_fixed" type="fixed">
0
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="RL_calf"/>
<child link="RL_foot"/>
</joint>
<link name="RL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
</inertial>
</link>
<transmission name="RL_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RL_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RL_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RL_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RL_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RL_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RL_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RL_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RL_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</robot>