state estimate tested
This commit is contained in:
parent
bfc5532ce3
commit
3149acf2ed
|
@ -11,7 +11,7 @@ Todo List:
|
|||
- [x] Unitree Guide Controller
|
||||
- [x] Gazebo Simulation
|
||||
- [x] Leg PD Controller
|
||||
- [ ] Contact Sensor
|
||||
- [x] Contact Sensor
|
||||
- [ ] OCS2 Legged Control
|
||||
|
||||
|
||||
|
|
|
@ -17,6 +17,7 @@ set(CONTROLLER_INCLUDE_DEPENDS
|
|||
ocs2_legged_robot_ros
|
||||
ocs2_self_collision
|
||||
control_input_msgs
|
||||
angles
|
||||
nav_msgs
|
||||
qpOASES
|
||||
)
|
||||
|
@ -55,12 +56,32 @@ target_include_directories(${PROJECT_NAME}
|
|||
PUBLIC
|
||||
${qpOASES_INCLUDE_DIR}
|
||||
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
|
||||
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
|
||||
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
|
||||
PRIVATE
|
||||
src)
|
||||
ament_target_dependencies(
|
||||
${PROJECT_NAME} PUBLIC
|
||||
${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)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
|
|
|
@ -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
|
|
@ -9,7 +9,6 @@
|
|||
#include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.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_ros/transform_broadcaster.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
|
@ -19,6 +18,7 @@ namespace ocs2::legged_robot {
|
|||
public:
|
||||
KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
|
||||
const PinocchioEndEffectorKinematics &eeKinematics,
|
||||
CtrlComponent &ctrl_component,
|
||||
const rclcpp_lifecycle::LifecycleNode::SharedPtr &node);
|
||||
|
||||
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);
|
||||
|
||||
protected:
|
||||
void updateFromTopic();
|
||||
|
||||
void callback(const nav_msgs::msg::Odometry::SharedPtr &msg);
|
||||
|
||||
nav_msgs::msg::Odometry getOdomMsg();
|
||||
|
||||
vector_t feetHeights_;
|
||||
|
@ -48,10 +44,5 @@ namespace ocs2::legged_robot {
|
|||
|
||||
matrix_t a_, b_, c_, q_, p_, r_;
|
||||
vector_t xHat_, ps_, vs_;
|
||||
|
||||
// Topic
|
||||
tf2::Transform world2odom_;
|
||||
std::string frameOdom_, frameGuess_;
|
||||
bool topicUpdated_;
|
||||
};
|
||||
} // namespace legged
|
||||
|
|
|
@ -16,6 +16,8 @@
|
|||
#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
|
||||
#include <rclcpp_lifecycle/lifecycle_node.hpp>
|
||||
|
||||
struct CtrlComponent;
|
||||
|
||||
namespace ocs2::legged_robot {
|
||||
class StateEstimateBase {
|
||||
public:
|
||||
|
@ -23,27 +25,27 @@ namespace ocs2::legged_robot {
|
|||
|
||||
StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
|
||||
const PinocchioEndEffectorKinematics &eeKinematics,
|
||||
CtrlComponent &ctrl_component,
|
||||
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,
|
||||
const vector3_t &linearAccelLocal,
|
||||
const matrix3_t &orientationCovariance, const matrix3_t &angularVelCovariance,
|
||||
const matrix3_t &linearAccelCovariance);
|
||||
virtual void updateImu();
|
||||
|
||||
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:
|
||||
void updateAngular(const vector3_t &zyx, const vector_t &angularVel);
|
||||
|
||||
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_;
|
||||
CentroidalModelInfo info_;
|
||||
|
@ -51,15 +53,13 @@ namespace ocs2::legged_robot {
|
|||
|
||||
vector3_t zyxOffset_ = vector3_t::Zero();
|
||||
vector_t rbdState_;
|
||||
contact_flag_t contactFlag_{};
|
||||
contact_flag_t contact_flag_{};
|
||||
Eigen::Quaternion<scalar_t> quat_;
|
||||
vector3_t angularVelLocal_, linearAccelLocal_;
|
||||
vector3_t angular_vel_local_, linear_accel_local_;
|
||||
matrix3_t orientationCovariance_, angularVelCovariance_, linearAccelCovariance_;
|
||||
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPub_;
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr posePub_;
|
||||
|
||||
rclcpp::Time lastPub_;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
|
||||
};
|
||||
|
||||
|
|
|
@ -27,43 +27,43 @@
|
|||
namespace ocs2::legged_robot {
|
||||
class LeggedInterface : public RobotInterface {
|
||||
public:
|
||||
LeggedInterface(const std::string &taskFile, const std::string &urdfFile, const std::string &referenceFile,
|
||||
bool useHardFrictionConeConstraint = false);
|
||||
LeggedInterface(const std::string &task_file, const std::string &urdf_file, const std::string &reference_file,
|
||||
bool use_hard_friction_cone_constraint = false);
|
||||
|
||||
~LeggedInterface() override = default;
|
||||
|
||||
virtual void setupOptimalControlProblem(const std::string &taskFile, const std::string &urdfFile,
|
||||
const std::string &referenceFile,
|
||||
virtual void setupOptimalControlProblem(const std::string &task_file, const std::string &urdf_file,
|
||||
const std::string &reference_file,
|
||||
bool verbose);
|
||||
|
||||
const OptimalControlProblem &getOptimalControlProblem() const override { return *problemPtr_; }
|
||||
const OptimalControlProblem &getOptimalControlProblem() const override { return *problem_ptr_; }
|
||||
|
||||
const ModelSettings &modelSettings() const { return modelSettings_; }
|
||||
const ddp::Settings &ddpSettings() const { return ddpSettings_; }
|
||||
const mpc::Settings &mpcSettings() const { return mpcSettings_; }
|
||||
const rollout::Settings &rolloutSettings() const { return rolloutSettings_; }
|
||||
const sqp::Settings &sqpSettings() { return sqpSettings_; }
|
||||
const ipm::Settings &ipmSettings() { return ipmSettings_; }
|
||||
const ModelSettings &modelSettings() const { return model_settings_; }
|
||||
const ddp::Settings &ddpSettings() const { return ddp_settings_; }
|
||||
const mpc::Settings &mpcSettings() const { return mpc_settings_; }
|
||||
const rollout::Settings &rolloutSettings() const { return rollout_settings_; }
|
||||
const sqp::Settings &sqpSettings() { return sqp_settings_; }
|
||||
const ipm::Settings &ipmSettings() { return ipm_settings_; }
|
||||
|
||||
const vector_t &getInitialState() const { return initialState_; }
|
||||
const RolloutBase &getRollout() const { return *rolloutPtr_; }
|
||||
PinocchioInterface &getPinocchioInterface() { return *pinocchioInterfacePtr_; }
|
||||
const CentroidalModelInfo &getCentroidalModelInfo() const { return centroidalModelInfo_; }
|
||||
PinocchioGeometryInterface &getGeometryInterface() { return *geometryInterfacePtr_; }
|
||||
const vector_t &getInitialState() const { return initial_state_; }
|
||||
const RolloutBase &getRollout() const { return *rollout_ptr_; }
|
||||
PinocchioInterface &getPinocchioInterface() { return *pinocchio_interface_ptr_; }
|
||||
const CentroidalModelInfo &getCentroidalModelInfo() const { return centroidal_model_info_; }
|
||||
PinocchioGeometryInterface &getGeometryInterface() { return *geometry_interface_ptr_; }
|
||||
|
||||
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 {
|
||||
return referenceManagerPtr_;
|
||||
return reference_manager_ptr_;
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void setupModel(const std::string &taskFile, const std::string &urdfFile,
|
||||
const std::string &referenceFile, bool verbose);
|
||||
virtual void setupModel(const std::string &task_file, const std::string &urdf_file,
|
||||
const std::string &reference_file);
|
||||
|
||||
virtual void setupReferenceManager(const std::string &taskFile, const std::string &urdfFile,
|
||||
const std::string &referenceFile,
|
||||
|
@ -91,36 +91,36 @@ namespace ocs2::legged_robot {
|
|||
const RelaxedBarrierPenalty::Config &
|
||||
barrierPenaltyConfig);
|
||||
|
||||
std::unique_ptr<EndEffectorKinematics<scalar_t> > getEeKinematicsPtr(const std::vector<std::string> &footNames,
|
||||
const std::string &modelName);
|
||||
std::unique_ptr<EndEffectorKinematics<scalar_t> > getEeKinematicsPtr(const std::vector<std::string> &foot_names,
|
||||
const std::string &model_name);
|
||||
|
||||
std::unique_ptr<StateInputConstraint> getZeroVelocityConstraint(
|
||||
const EndEffectorKinematics<scalar_t> &eeKinematics,
|
||||
size_t contactPointIndex);
|
||||
const EndEffectorKinematics<scalar_t> &end_effector_kinematics,
|
||||
size_t contact_point_index);
|
||||
|
||||
std::unique_ptr<StateCost> getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface,
|
||||
const std::string &taskFile,
|
||||
const std::string &prefix, bool verbose);
|
||||
|
||||
ModelSettings modelSettings_;
|
||||
mpc::Settings mpcSettings_;
|
||||
ddp::Settings ddpSettings_;
|
||||
sqp::Settings sqpSettings_;
|
||||
ipm::Settings ipmSettings_;
|
||||
const bool useHardFrictionConeConstraint_;
|
||||
ModelSettings model_settings_;
|
||||
mpc::Settings mpc_settings_;
|
||||
ddp::Settings ddp_settings_;
|
||||
sqp::Settings sqp_settings_;
|
||||
ipm::Settings ipm_settings_;
|
||||
const bool use_hard_friction_cone_constraint_;
|
||||
|
||||
std::unique_ptr<PinocchioInterface> pinocchioInterfacePtr_;
|
||||
CentroidalModelInfo centroidalModelInfo_;
|
||||
std::unique_ptr<PinocchioGeometryInterface> geometryInterfacePtr_;
|
||||
std::unique_ptr<PinocchioInterface> pinocchio_interface_ptr_;
|
||||
CentroidalModelInfo centroidal_model_info_;
|
||||
std::unique_ptr<PinocchioGeometryInterface> geometry_interface_ptr_;
|
||||
|
||||
std::unique_ptr<OptimalControlProblem> problemPtr_;
|
||||
std::shared_ptr<SwitchedModelReferenceManager> referenceManagerPtr_;
|
||||
std::unique_ptr<OptimalControlProblem> problem_ptr_;
|
||||
std::shared_ptr<SwitchedModelReferenceManager> reference_manager_ptr_;
|
||||
|
||||
rollout::Settings rolloutSettings_;
|
||||
std::unique_ptr<RolloutBase> rolloutPtr_;
|
||||
std::unique_ptr<Initializer> initializerPtr_;
|
||||
rollout::Settings rollout_settings_;
|
||||
std::unique_ptr<RolloutBase> rollout_ptr_;
|
||||
std::unique_ptr<Initializer> initializer_ptr_;
|
||||
|
||||
vector_t initialState_;
|
||||
vector_t initial_state_;
|
||||
};
|
||||
} // namespace legged
|
||||
|
||||
|
|
|
@ -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>
|
|
@ -15,6 +15,7 @@
|
|||
<depend>control_input_msgs</depend>
|
||||
<depend>qpOASES</depend>
|
||||
<depend>ocs2_self_collision</depend>
|
||||
<depend>angles</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
|
|
@ -12,13 +12,116 @@
|
|||
#include <ocs2_quadruped_controller/wbc/WeightedWbc.h>
|
||||
#include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h>
|
||||
#include <ocs2_sqp/SqpMpc.h>
|
||||
#include <angles/angles.h>
|
||||
|
||||
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() {
|
||||
// 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;
|
||||
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();
|
||||
setupMrt();
|
||||
|
||||
|
@ -47,7 +150,75 @@ namespace ocs2::legged_robot {
|
|||
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_->setupOptimalControlProblem(task_file_, urdf_file_, reference_file_, verbose_);
|
||||
}
|
||||
|
@ -60,18 +231,21 @@ namespace ocs2::legged_robot {
|
|||
legged_interface_->getCentroidalModelInfo());
|
||||
|
||||
const std::string robotName = "legged_robot";
|
||||
|
||||
// Todo Handle Gait Receive.
|
||||
// Gait receiver
|
||||
auto gaitReceiverPtr =
|
||||
std::make_shared<GaitReceiver>(this->get_node(),
|
||||
legged_interface_->getSwitchedModelReferenceManagerPtr()->
|
||||
getGaitSchedule(), robotName);
|
||||
// auto gaitReceiverPtr =
|
||||
// std::make_shared<GaitReceiver>(this->get_node(),
|
||||
// legged_interface_->getSwitchedModelReferenceManagerPtr()->
|
||||
// getGaitSchedule(), robotName);
|
||||
|
||||
// ROS ReferenceManager
|
||||
auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>(
|
||||
robotName, legged_interface_->getReferenceManagerPtr());
|
||||
rosReferenceManagerPtr->subscribe(this->get_node());
|
||||
mpc_->getSolverPtr()->addSynchronizedModule(gaitReceiverPtr);
|
||||
mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);
|
||||
observationPublisher_ = nh.advertise<ocs2_msgs::msg::mpc_observation>(robotName + "_mpc_observation", 1);
|
||||
// auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>(
|
||||
// robotName, legged_interface_->getReferenceManagerPtr());
|
||||
// rosReferenceManagerPtr->subscribe(this->get_node());
|
||||
// mpc_->getSolverPtr()->addSynchronizedModule(gaitReceiverPtr);
|
||||
// mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);
|
||||
// observationPublisher_ = nh.advertise<ocs2_msgs::msg::mpc_observation>(robotName + "_mpc_observation", 1);
|
||||
}
|
||||
|
||||
void Ocs2QuadrupedController::setupMrt() {
|
||||
|
@ -80,11 +254,11 @@ namespace ocs2::legged_robot {
|
|||
mpcTimer_.reset();
|
||||
|
||||
controllerRunning_ = true;
|
||||
mpcThread_ = std::thread([&]() {
|
||||
mpcThread_ = std::thread([&] {
|
||||
while (controllerRunning_) {
|
||||
try {
|
||||
executeAndSleep(
|
||||
[&]() {
|
||||
[&] {
|
||||
if (mpcRunning_) {
|
||||
mpcTimer_.startTimer();
|
||||
mpcMrtInterface_->advanceMpc();
|
||||
|
@ -102,10 +276,13 @@ namespace ocs2::legged_robot {
|
|||
}
|
||||
|
||||
void Ocs2QuadrupedController::setupStateEstimate() {
|
||||
stateEstimate_ = std::make_shared<KalmanFilterEstimate>(legged_interface_->getPinocchioInterface(),
|
||||
ctrl_comp_.estimator_ = std::make_shared<KalmanFilterEstimate>(legged_interface_->getPinocchioInterface(),
|
||||
legged_interface_->getCentroidalModelInfo(),
|
||||
*eeKinematicsPtr_, this->get_node());
|
||||
dynamic_cast<KalmanFilterEstimate &>(*stateEstimate_).loadSettings(task_file_, verbose_);
|
||||
*eeKinematicsPtr_, ctrl_comp_, this->get_node());
|
||||
dynamic_cast<KalmanFilterEstimate &>(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_);
|
||||
currentObservation_.time = 0;
|
||||
}
|
||||
}
|
||||
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
PLUGINLIB_EXPORT_CLASS(ocs2::legged_robot::Ocs2QuadrupedController, controller_interface::ControllerInterface);
|
|
@ -4,13 +4,16 @@
|
|||
|
||||
#ifndef OCS2QUADRUPEDCONTROLLER_H
|
||||
#define OCS2QUADRUPEDCONTROLLER_H
|
||||
#include "SafetyChecker.h"
|
||||
|
||||
#include <controller_interface/controller_interface.hpp>
|
||||
#include <ocs2_quadruped_controller/interface/LeggedInterface.h>
|
||||
#include <ocs2_mpc/MPC_MRT_Interface.h>
|
||||
#include <control_input_msgs/msg/inputs.hpp>
|
||||
#include <ocs2_centroidal_model/CentroidalModelRbdConversions.h>
|
||||
#include <ocs2_mpc/MPC_MRT_Interface.h>
|
||||
#include <ocs2_quadruped_controller/estimator/StateEstimateBase.h>
|
||||
#include <ocs2_quadruped_controller/interface/LeggedInterface.h>
|
||||
#include <ocs2_quadruped_controller/wbc/WbcBase.h>
|
||||
#include "SafetyChecker.h"
|
||||
#include "ocs2_quadruped_controller/control/CtrlComponent.h"
|
||||
|
||||
namespace ocs2::legged_robot {
|
||||
class Ocs2QuadrupedController final : public controller_interface::ControllerInterface {
|
||||
|
@ -56,7 +59,7 @@ namespace ocs2::legged_robot {
|
|||
const rclcpp_lifecycle::State &previous_state) override;
|
||||
|
||||
protected:
|
||||
void setUpLeggedInterface();
|
||||
void setupLeggedInterface();
|
||||
|
||||
void setupMpc();
|
||||
|
||||
|
@ -64,6 +67,38 @@ namespace ocs2::legged_robot {
|
|||
|
||||
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_;
|
||||
|
||||
std::string task_file_;
|
||||
|
|
|
@ -15,18 +15,20 @@
|
|||
namespace ocs2::legged_robot {
|
||||
KalmanFilterEstimate::KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
|
||||
const PinocchioEndEffectorKinematics &eeKinematics,
|
||||
CtrlComponent &ctrl_component,
|
||||
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),
|
||||
dimContacts_(3 * numContacts_),
|
||||
numState_(6 + dimContacts_),
|
||||
numObserve_(2 * dimContacts_ + numContacts_),
|
||||
topicUpdated_(false) {
|
||||
numObserve_(2 * dimContacts_ + numContacts_) {
|
||||
xHat_.setZero(numState_);
|
||||
ps_.setZero(dimContacts_);
|
||||
vs_.setZero(dimContacts_);
|
||||
a_.setIdentity(numState_, numState_);
|
||||
b_.setZero(numState_, 3);
|
||||
|
||||
matrix_t c1(3, 6), c2(3, 6);
|
||||
c1 << matrix3_t::Identity(), matrix3_t::Zero();
|
||||
c2 << matrix3_t::Zero(), matrix3_t::Identity();
|
||||
|
@ -44,11 +46,13 @@ namespace ocs2::legged_robot {
|
|||
feetHeights_.setZero(numContacts_);
|
||||
|
||||
eeKinematics_->setPinocchioInterface(pinocchioInterface_);
|
||||
|
||||
world2odom_.setRotation(tf2::Quaternion::getIdentity());
|
||||
}
|
||||
|
||||
vector_t KalmanFilterEstimate::update(const rclcpp::Time &time, const rclcpp::Duration &period) {
|
||||
updateJointStates();
|
||||
updateContact();
|
||||
updateImu();
|
||||
|
||||
scalar_t dt = period.seconds();
|
||||
a_.block(0, 3, 3, 3) = 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 rIndex2 = dimContacts_ + i1;
|
||||
int rIndex3 = 2 * dimContacts_ + i;
|
||||
bool isContact = contactFlag_[i];
|
||||
bool isContact = contact_flag_[i];
|
||||
|
||||
scalar_t high_suspect_number(100);
|
||||
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 accel = getRotationMatrixFromZyxEulerAngles(quatToZyx(quat_)) * linearAccelLocal_ + g;
|
||||
vector3_t accel = getRotationMatrixFromZyxEulerAngles(quatToZyx(quat_)) * linear_accel_local_ + g;
|
||||
|
||||
vector_t y(numObserve_);
|
||||
y << ps_, vs_, feetHeights_;
|
||||
|
@ -142,11 +146,6 @@ namespace ocs2::legged_robot {
|
|||
// p_.block(0, 0, 2, 2) /= 10.;
|
||||
// }
|
||||
|
||||
if (topicUpdated_) {
|
||||
updateFromTopic();
|
||||
topicUpdated_ = false;
|
||||
}
|
||||
|
||||
updateLinear(xHat_.segment<3>(0), xHat_.segment<3>(3));
|
||||
|
||||
auto odom = getOdomMsg();
|
||||
|
@ -179,9 +178,9 @@ namespace ocs2::legged_robot {
|
|||
odom.twist.twist.linear.x = twist.x();
|
||||
odom.twist.twist.linear.y = twist.y();
|
||||
odom.twist.twist.linear.z = twist.z();
|
||||
odom.twist.twist.angular.x = angularVelLocal_.x();
|
||||
odom.twist.twist.angular.y = angularVelLocal_.y();
|
||||
odom.twist.twist.angular.z = angularVelLocal_.z();
|
||||
odom.twist.twist.angular.x = angular_vel_local_.x();
|
||||
odom.twist.twist.angular.y = angular_vel_local_.y();
|
||||
odom.twist.twist.angular.z = angular_vel_local_.z();
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
for (int j = 0; j < 3; ++j) {
|
||||
odom.twist.covariance[i * 6 + j] = p_.block<3, 3>(3, 3)(i, j);
|
||||
|
|
|
@ -9,39 +9,71 @@
|
|||
#include <ocs2_robotic_tools/common/RotationDerivativesTransforms.h>
|
||||
|
||||
#include <memory>
|
||||
#include "ocs2_quadruped_controller/control/CtrlComponent.h"
|
||||
|
||||
namespace ocs2::legged_robot {
|
||||
using namespace legged_robot;
|
||||
|
||||
StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
|
||||
const PinocchioEndEffectorKinematics &eeKinematics,
|
||||
CtrlComponent &ctrl_component,
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr node)
|
||||
: pinocchioInterface_(std::move(pinocchioInterface)),
|
||||
: ctrl_component_(ctrl_component),
|
||||
pinocchioInterface_(std::move(pinocchioInterface)),
|
||||
info_(std::move(info)),
|
||||
eeKinematics_(eeKinematics.clone()),
|
||||
rbdState_(vector_t::Zero(2 * info_.generalizedCoordinatesNum)), node_(std::move(node)) {
|
||||
odomPub_ = node_->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
|
||||
posePub_ = node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("pose", 10);
|
||||
odom_pub_ = node_->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
|
||||
pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("pose", 10);
|
||||
}
|
||||
|
||||
void StateEstimateBase::updateJointStates(const vector_t &jointPos, const vector_t &jointVel) {
|
||||
rbdState_.segment(6, info_.actuatedDofNum) = jointPos;
|
||||
rbdState_.segment(6 + info_.generalizedCoordinatesNum, info_.actuatedDofNum) = jointVel;
|
||||
void StateEstimateBase::updateJointStates() {
|
||||
const size_t size = ctrl_component_.joint_effort_state_interface_.size();
|
||||
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();
|
||||
}
|
||||
|
||||
void StateEstimateBase::updateImu(const Eigen::Quaternion<scalar_t> &quat, const vector3_t &angularVelLocal,
|
||||
const vector3_t &linearAccelLocal, const matrix3_t &orientationCovariance,
|
||||
const matrix3_t &angularVelCovariance, const matrix3_t &linearAccelCovariance) {
|
||||
quat_ = quat;
|
||||
angularVelLocal_ = angularVelLocal;
|
||||
linearAccelLocal_ = linearAccelLocal;
|
||||
orientationCovariance_ = orientationCovariance;
|
||||
angularVelCovariance_ = angularVelCovariance;
|
||||
linearAccelCovariance_ = linearAccelCovariance;
|
||||
rbdState_.segment(6, info_.actuatedDofNum) = joint_pos;
|
||||
rbdState_.segment(6 + info_.generalizedCoordinatesNum, info_.actuatedDofNum) = joint_vel;
|
||||
}
|
||||
|
||||
vector3_t zyx = quatToZyx(quat) - zyxOffset_;
|
||||
vector3_t angularVelGlobal = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives<scalar_t>(
|
||||
zyx, getEulerAnglesZyxDerivativesFromLocalAngularVelocity<scalar_t>(quatToZyx(quat), angularVelLocal));
|
||||
void StateEstimateBase::updateContact() {
|
||||
const size_t size = ctrl_component_.foot_force_state_interface_.size();
|
||||
for (int i = 0; i < size; i++) {
|
||||
contact_flag_[i] = ctrl_component_.foot_force_state_interface_[i].get().get_value() > 1;
|
||||
}
|
||||
}
|
||||
|
||||
void StateEstimateBase::updateImu() {
|
||||
quat_ = {
|
||||
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);
|
||||
}
|
||||
|
||||
|
@ -55,20 +87,13 @@ namespace ocs2::legged_robot {
|
|||
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;
|
||||
scalar_t publishRate = 200;
|
||||
// if (lastPub_ + rclcpp::Duration(1. / publishRate) < time) {
|
||||
// lastPub_ = time;
|
||||
// if (odomPub_->trylock()) {
|
||||
// odomPub_->msg_ = odom;
|
||||
// odomPub_->unlockAndPublish();
|
||||
// }
|
||||
// if (posePub_->trylock()) {
|
||||
// posePub_->msg_.header = odom.header;
|
||||
// posePub_->msg_.pose = odom.pose;
|
||||
// posePub_->unlockAndPublish();
|
||||
// }
|
||||
// }
|
||||
odom_pub_->publish(odom);
|
||||
|
||||
geometry_msgs::msg::PoseWithCovarianceStamped pose;
|
||||
pose.header = odom.header;
|
||||
pose.pose.pose = odom.pose.pose;
|
||||
pose_pub_->publish(pose);
|
||||
}
|
||||
} // namespace legged
|
||||
|
|
|
@ -2,6 +2,7 @@
|
|||
// Created by qiayuan on 2022/7/16.
|
||||
//
|
||||
|
||||
#include <memory>
|
||||
#include <pinocchio/fwd.hpp> // forward declarations must be included first.
|
||||
|
||||
#include <pinocchio/algorithm/frames.hpp>
|
||||
|
@ -35,166 +36,160 @@
|
|||
#include <boost/filesystem/path.hpp>
|
||||
|
||||
namespace ocs2::legged_robot {
|
||||
LeggedInterface::LeggedInterface(const std::string &taskFile, const std::string &urdfFile,
|
||||
const std::string &referenceFile,
|
||||
bool useHardFrictionConeConstraint)
|
||||
: useHardFrictionConeConstraint_(useHardFrictionConeConstraint) {
|
||||
LeggedInterface::LeggedInterface(const std::string &task_file, const std::string &urdf_file,
|
||||
const std::string &reference_file,
|
||||
const bool use_hard_friction_cone_constraint)
|
||||
: use_hard_friction_cone_constraint_(use_hard_friction_cone_constraint) {
|
||||
// check that task file exists
|
||||
boost::filesystem::path taskFilePath(taskFile);
|
||||
if (boost::filesystem::exists(taskFilePath)) {
|
||||
std::cerr << "[LeggedInterface] Loading task file: " << taskFilePath << std::endl;
|
||||
if (const boost::filesystem::path task_file_path(task_file); exists(task_file_path)) {
|
||||
std::cerr << "[LeggedInterface] Loading task file: " << task_file_path << std::endl;
|
||||
} 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
|
||||
boost::filesystem::path urdfFilePath(urdfFile);
|
||||
if (boost::filesystem::exists(urdfFilePath)) {
|
||||
std::cerr << "[LeggedInterface] Loading Pinocchio model from: " << urdfFilePath << std::endl;
|
||||
if (const boost::filesystem::path urdf_file_path(urdf_file); exists(urdf_file_path)) {
|
||||
std::cerr << "[LeggedInterface] Loading Pinocchio model from: " << urdf_file_path << std::endl;
|
||||
} 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
|
||||
boost::filesystem::path referenceFilePath(referenceFile);
|
||||
if (boost::filesystem::exists(referenceFilePath)) {
|
||||
std::cerr << "[LeggedInterface] Loading target command settings from: " << referenceFilePath << std::endl;
|
||||
if (const boost::filesystem::path reference_file_path(reference_file); exists(reference_file_path)) {
|
||||
std::cerr << "[LeggedInterface] Loading target command settings from: " << reference_file_path << std::endl;
|
||||
} else {
|
||||
throw std::invalid_argument(
|
||||
"[LeggedInterface] targetCommand file not found: " + referenceFilePath.string());
|
||||
"[LeggedInterface] targetCommand file not found: " + reference_file_path.string());
|
||||
}
|
||||
|
||||
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
|
||||
modelSettings_ = loadModelSettings(taskFile, "model_settings", verbose);
|
||||
mpcSettings_ = mpc::loadSettings(taskFile, "mpc", verbose);
|
||||
ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose);
|
||||
sqpSettings_ = sqp::loadSettings(taskFile, "sqp", verbose);
|
||||
ipmSettings_ = ipm::loadSettings(taskFile, "ipm", verbose);
|
||||
rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose);
|
||||
model_settings_ = loadModelSettings(task_file, "model_settings", verbose);
|
||||
|
||||
// Todo : load settings from ros param
|
||||
model_settings_.jointNames = {
|
||||
"FF_hip_joint", "FL_thigh_joint", "FL_calf_joint",
|
||||
"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 &taskFile, const std::string &urdfFile,
|
||||
const std::string &referenceFile,
|
||||
bool verbose) {
|
||||
setupModel(taskFile, urdfFile, referenceFile, verbose);
|
||||
|
||||
void LeggedInterface::setupOptimalControlProblem(const std::string &task_file, const std::string &urdf_file,
|
||||
const std::string &reference_file,
|
||||
const bool verbose) {
|
||||
setupModel(task_file, urdf_file, reference_file);
|
||||
|
||||
// Initial state
|
||||
initialState_.setZero(centroidalModelInfo_.stateDim);
|
||||
loadData::loadEigenMatrix(taskFile, "initialState", initialState_);
|
||||
initial_state_.setZero(centroidal_model_info_.stateDim);
|
||||
loadData::loadEigenMatrix(task_file, "initialState", initial_state_);
|
||||
|
||||
setupReferenceManager(taskFile, urdfFile, referenceFile, verbose);
|
||||
setupReferenceManager(task_file, urdf_file, reference_file, verbose);
|
||||
|
||||
// Optimal control problem
|
||||
problemPtr_ = std::make_unique<OptimalControlProblem>();
|
||||
problem_ptr_ = std::make_unique<OptimalControlProblem>();
|
||||
|
||||
// Dynamics
|
||||
std::unique_ptr<SystemDynamicsBase> dynamicsPtr;
|
||||
dynamicsPtr = std::make_unique<LeggedRobotDynamicsAD>(*pinocchioInterfacePtr_, centroidalModelInfo_, "dynamics",
|
||||
modelSettings_);
|
||||
problemPtr_->dynamicsPtr = std::move(dynamicsPtr);
|
||||
std::unique_ptr<SystemDynamicsBase> dynamicsPtr = std::make_unique<LeggedRobotDynamicsAD>(
|
||||
*pinocchio_interface_ptr_, centroidal_model_info_, "dynamics",
|
||||
model_settings_);
|
||||
problem_ptr_->dynamicsPtr = std::move(dynamicsPtr);
|
||||
|
||||
// Cost terms
|
||||
problemPtr_->costPtr->add("baseTrackingCost", getBaseTrackingCost(taskFile, centroidalModelInfo_, verbose));
|
||||
problem_ptr_->costPtr->add("baseTrackingCost", getBaseTrackingCost(task_file, centroidal_model_info_, verbose));
|
||||
|
||||
// Constraint terms
|
||||
// friction cone settings
|
||||
scalar_t frictionCoefficient = 0.7;
|
||||
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++) {
|
||||
const std::string &footName = modelSettings_.contactNames3DoF[i];
|
||||
for (size_t i = 0; i < centroidal_model_info_.numThreeDofContacts; i++) {
|
||||
const std::string &footName = model_settings_.contactNames3DoF[i];
|
||||
std::unique_ptr<EndEffectorKinematics<scalar_t> > eeKinematicsPtr =
|
||||
getEeKinematicsPtr({footName}, footName);
|
||||
|
||||
if (useHardFrictionConeConstraint_) {
|
||||
problemPtr_->inequalityConstraintPtr->add(footName + "_frictionCone",
|
||||
if (use_hard_friction_cone_constraint_) {
|
||||
problem_ptr_->inequalityConstraintPtr->add(footName + "_frictionCone",
|
||||
getFrictionConeConstraint(i, frictionCoefficient));
|
||||
} else {
|
||||
problemPtr_->softConstraintPtr->add(footName + "_frictionCone",
|
||||
problem_ptr_->softConstraintPtr->add(footName + "_frictionCone",
|
||||
getFrictionConeSoftConstraint(
|
||||
i, frictionCoefficient, barrierPenaltyConfig));
|
||||
}
|
||||
problemPtr_->equalityConstraintPtr->add(footName + "_zeroForce", std::unique_ptr<StateInputConstraint>(
|
||||
new ZeroForceConstraint(
|
||||
*referenceManagerPtr_, i, centroidalModelInfo_)));
|
||||
problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity",
|
||||
problem_ptr_->equalityConstraintPtr->add(footName + "_zeroForce", std::make_unique<ZeroForceConstraint>(
|
||||
*reference_manager_ptr_, i, centroidal_model_info_));
|
||||
problem_ptr_->equalityConstraintPtr->add(footName + "_zeroVelocity",
|
||||
getZeroVelocityConstraint(*eeKinematicsPtr, i));
|
||||
problemPtr_->equalityConstraintPtr->add(
|
||||
problem_ptr_->equalityConstraintPtr->add(
|
||||
footName + "_normalVelocity",
|
||||
std::unique_ptr<StateInputConstraint>(
|
||||
new NormalVelocityConstraintCppAd(*referenceManagerPtr_, *eeKinematicsPtr, i)));
|
||||
std::make_unique<NormalVelocityConstraintCppAd>(*reference_manager_ptr_, *eeKinematicsPtr, i));
|
||||
}
|
||||
|
||||
// Self-collision avoidance constraint
|
||||
problemPtr_->stateSoftConstraintPtr->add("selfCollision",
|
||||
problem_ptr_->stateSoftConstraintPtr->add("selfCollision",
|
||||
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
|
||||
rolloutPtr_ = std::make_unique<TimeTriggeredRollout>(*problemPtr_->dynamicsPtr, rolloutSettings_);
|
||||
rollout_ptr_ = std::make_unique<TimeTriggeredRollout>(*problem_ptr_->dynamicsPtr, rollout_settings_);
|
||||
|
||||
// Initialization
|
||||
constexpr bool extendNormalizedNomentum = true;
|
||||
initializerPtr_ = std::make_unique<LeggedRobotInitializer>(centroidalModelInfo_, *referenceManagerPtr_,
|
||||
initializer_ptr_ = std::make_unique<LeggedRobotInitializer>(centroidal_model_info_, *reference_manager_ptr_,
|
||||
extendNormalizedNomentum);
|
||||
}
|
||||
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
void LeggedInterface::setupModel(const std::string &taskFile, const std::string &urdfFile,
|
||||
const std::string &referenceFile,
|
||||
bool /*verbose*/) {
|
||||
|
||||
void LeggedInterface::setupModel(const std::string &task_file, const std::string &urdf_file,
|
||||
const std::string &reference_file) {
|
||||
// PinocchioInterface
|
||||
pinocchioInterfacePtr_ =
|
||||
pinocchio_interface_ptr_ =
|
||||
std::make_unique<PinocchioInterface>(
|
||||
centroidal_model::createPinocchioInterface(urdfFile, modelSettings_.jointNames));
|
||||
centroidal_model::createPinocchioInterface(urdf_file, model_settings_.jointNames));
|
||||
|
||||
// CentroidalModelInfo
|
||||
centroidalModelInfo_ = centroidal_model::createCentroidalModelInfo(
|
||||
*pinocchioInterfacePtr_, centroidal_model::loadCentroidalType(taskFile),
|
||||
centroidal_model::loadDefaultJointState(pinocchioInterfacePtr_->getModel().nq - 6, referenceFile),
|
||||
modelSettings_.contactNames3DoF,
|
||||
modelSettings_.contactNames6DoF);
|
||||
centroidal_model_info_ = centroidal_model::createCentroidalModelInfo(
|
||||
*pinocchio_interface_ptr_, centroidal_model::loadCentroidalType(task_file),
|
||||
centroidal_model::loadDefaultJointState(pinocchio_interface_ptr_->getModel().nq - 6, reference_file),
|
||||
model_settings_.contactNames3DoF,
|
||||
model_settings_.contactNames6DoF);
|
||||
}
|
||||
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
|
||||
void LeggedInterface::setupReferenceManager(const std::string &taskFile, const std::string &urdfFile,
|
||||
const std::string &referenceFile,
|
||||
bool verbose) {
|
||||
auto swingTrajectoryPlanner =
|
||||
std::make_unique<SwingTrajectoryPlanner>(
|
||||
loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4);
|
||||
referenceManagerPtr_ =
|
||||
reference_manager_ptr_ =
|
||||
std::make_shared<SwitchedModelReferenceManager>(loadGaitSchedule(referenceFile, verbose),
|
||||
std::move(swingTrajectoryPlanner));
|
||||
}
|
||||
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
|
||||
void LeggedInterface::setupPreComputation(const std::string &taskFile, const std::string &urdfFile,
|
||||
const std::string &referenceFile,
|
||||
bool verbose) {
|
||||
problemPtr_->preComputationPtr = std::make_unique<LeggedRobotPreComputation>(
|
||||
*pinocchioInterfacePtr_, centroidalModelInfo_, *referenceManagerPtr_->getSwingTrajectoryPlanner(),
|
||||
modelSettings_);
|
||||
problem_ptr_->preComputationPtr = std::make_unique<LeggedRobotPreComputation>(
|
||||
*pinocchio_interface_ptr_, centroidal_model_info_, *reference_manager_ptr_->getSwingTrajectoryPlanner(),
|
||||
model_settings_);
|
||||
}
|
||||
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
|
||||
std::shared_ptr<GaitSchedule> LeggedInterface::loadGaitSchedule(const std::string &file, bool verbose) const {
|
||||
const auto initModeSchedule = loadModeSchedule(file, "initialModeSchedule", false);
|
||||
const auto defaultModeSequenceTemplate = loadModeSequenceTemplate(file, "defaultModeSequenceTemplate", false);
|
||||
|
@ -221,25 +216,23 @@ namespace ocs2::legged_robot {
|
|||
}
|
||||
|
||||
return std::make_shared<GaitSchedule>(initModeSchedule, defaultModeSequenceTemplate,
|
||||
modelSettings_.phaseTransitionStanceTime);
|
||||
model_settings_.phaseTransitionStanceTime);
|
||||
}
|
||||
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
|
||||
matrix_t LeggedInterface::initializeInputCostWeight(const std::string &taskFile, const CentroidalModelInfo &info) {
|
||||
const size_t totalContactDim = 3 * info.numThreeDofContacts;
|
||||
|
||||
const auto &model = pinocchioInterfacePtr_->getModel();
|
||||
auto &data = pinocchioInterfacePtr_->getData();
|
||||
const auto q = centroidal_model::getGeneralizedCoordinates(initialState_, centroidalModelInfo_);
|
||||
pinocchio::computeJointJacobians(model, data, q);
|
||||
pinocchio::updateFramePlacements(model, data);
|
||||
const auto &model = pinocchio_interface_ptr_->getModel();
|
||||
auto &data = pinocchio_interface_ptr_->getData();
|
||||
const auto q = centroidal_model::getGeneralizedCoordinates(initial_state_, centroidal_model_info_);
|
||||
computeJointJacobians(model, data, q);
|
||||
updateFramePlacements(model, data);
|
||||
|
||||
matrix_t base2feetJac(totalContactDim, info.actuatedDofNum);
|
||||
for (size_t i = 0; i < info.numThreeDofContacts; i++) {
|
||||
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);
|
||||
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;
|
||||
}
|
||||
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
|
||||
std::unique_ptr<StateInputCost> LeggedInterface::getBaseTrackingCost(
|
||||
const std::string &taskFile, const CentroidalModelInfo &info,
|
||||
bool verbose) {
|
||||
|
@ -274,16 +265,14 @@ namespace ocs2::legged_robot {
|
|||
}
|
||||
|
||||
return std::make_unique<LeggedRobotStateInputQuadraticCost>(std::move(Q), std::move(R), info,
|
||||
*referenceManagerPtr_);
|
||||
*reference_manager_ptr_);
|
||||
}
|
||||
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
|
||||
std::pair<scalar_t, RelaxedBarrierPenalty::Config> LeggedInterface::loadFrictionConeSettings(
|
||||
const std::string &taskFile, bool verbose) {
|
||||
boost::property_tree::ptree pt;
|
||||
boost::property_tree::read_info(taskFile, pt);
|
||||
read_info(taskFile, pt);
|
||||
const std::string prefix = "frictionConeSoftConstraint.";
|
||||
|
||||
scalar_t frictionCoefficient = 1.0;
|
||||
|
@ -302,19 +291,16 @@ namespace ocs2::legged_robot {
|
|||
return {frictionCoefficient, barrierPenaltyConfig};
|
||||
}
|
||||
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
|
||||
std::unique_ptr<StateInputConstraint> LeggedInterface::getFrictionConeConstraint(
|
||||
size_t contactPointIndex, scalar_t frictionCoefficient) {
|
||||
FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient);
|
||||
return std::make_unique<FrictionConeConstraint>(*referenceManagerPtr_, frictionConeConConfig, contactPointIndex,
|
||||
centroidalModelInfo_);
|
||||
return std::make_unique<FrictionConeConstraint>(*reference_manager_ptr_, frictionConeConConfig,
|
||||
contactPointIndex,
|
||||
centroidal_model_info_);
|
||||
}
|
||||
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
|
||||
std::unique_ptr<StateInputCost> LeggedInterface::getFrictionConeSoftConstraint(
|
||||
size_t contactPointIndex, scalar_t frictionCoefficient,
|
||||
const RelaxedBarrierPenalty::Config &barrierPenaltyConfig) {
|
||||
|
@ -323,39 +309,35 @@ namespace ocs2::legged_robot {
|
|||
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);
|
||||
auto velocityUpdateCallback = [&infoCppAd](const ad_vector_t &state,
|
||||
PinocchioInterfaceCppAd &pinocchioInterfaceAd) {
|
||||
const ad_vector_t q = centroidal_model::getGeneralizedCoordinates(state, infoCppAd);
|
||||
updateCentroidalDynamics(pinocchioInterfaceAd, infoCppAd, q);
|
||||
};
|
||||
eeKinematicsPtr.reset(new PinocchioEndEffectorKinematicsCppAd(*pinocchioInterfacePtr_, pinocchioMappingCppAd,
|
||||
footNames,
|
||||
centroidalModelInfo_.stateDim,
|
||||
centroidalModelInfo_.inputDim,
|
||||
velocityUpdateCallback, modelName,
|
||||
modelSettings_.modelFolderCppAd,
|
||||
modelSettings_.recompileLibrariesCppAd,
|
||||
modelSettings_.verboseCppAd));
|
||||
std::unique_ptr<EndEffectorKinematics<scalar_t> > end_effector_kinematics = std::make_unique<
|
||||
PinocchioEndEffectorKinematicsCppAd>(
|
||||
*pinocchio_interface_ptr_, pinocchioMappingCppAd,
|
||||
foot_names,
|
||||
centroidal_model_info_.stateDim,
|
||||
centroidal_model_info_.inputDim,
|
||||
velocityUpdateCallback, model_name,
|
||||
model_settings_.modelFolderCppAd,
|
||||
model_settings_.recompileLibrariesCppAd,
|
||||
model_settings_.verboseCppAd);
|
||||
|
||||
return eeKinematicsPtr;
|
||||
return end_effector_kinematics;
|
||||
}
|
||||
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
|
||||
std::unique_ptr<StateInputConstraint> LeggedInterface::getZeroVelocityConstraint(
|
||||
const EndEffectorKinematics<scalar_t> &eeKinematics,
|
||||
size_t contactPointIndex) {
|
||||
const EndEffectorKinematics<scalar_t> &end_effector_kinematics,
|
||||
const size_t contact_point_index) {
|
||||
auto eeZeroVelConConfig = [](scalar_t positionErrorGain) {
|
||||
EndEffectorLinearConstraint::Config config;
|
||||
config.b.setZero(3);
|
||||
|
@ -366,14 +348,12 @@ namespace ocs2::legged_robot {
|
|||
}
|
||||
return config;
|
||||
};
|
||||
return std::unique_ptr<StateInputConstraint>(new ZeroVelocityConstraintCppAd(
|
||||
*referenceManagerPtr_, eeKinematics, contactPointIndex,
|
||||
eeZeroVelConConfig(modelSettings_.positionErrorGain)));
|
||||
return std::make_unique<ZeroVelocityConstraintCppAd>(
|
||||
*reference_manager_ptr_, end_effector_kinematics, contact_point_index,
|
||||
eeZeroVelConConfig(model_settings_.positionErrorGain));
|
||||
}
|
||||
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
/******************************************************************************************************/
|
||||
|
||||
std::unique_ptr<StateCost> LeggedInterface::getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface,
|
||||
const std::string &taskFile,
|
||||
const std::string &prefix,
|
||||
|
@ -385,7 +365,7 @@ namespace ocs2::legged_robot {
|
|||
scalar_t minimumDistance = 0.0;
|
||||
|
||||
boost::property_tree::ptree pt;
|
||||
boost::property_tree::read_info(taskFile, pt);
|
||||
read_info(taskFile, pt);
|
||||
if (verbose) {
|
||||
std::cerr << "\n #### SelfCollision Settings: ";
|
||||
std::cerr << "\n #### =============================================================================\n";
|
||||
|
@ -396,16 +376,16 @@ namespace ocs2::legged_robot {
|
|||
loadData::loadStdVectorOfPair(taskFile, prefix + ".collisionObjectPairs", collisionObjectPairs, verbose);
|
||||
loadData::loadStdVectorOfPair(taskFile, prefix + ".collisionLinkPairs", collisionLinkPairs, verbose);
|
||||
|
||||
geometryInterfacePtr_ = std::make_unique<PinocchioGeometryInterface>(
|
||||
geometry_interface_ptr_ = std::make_unique<PinocchioGeometryInterface>(
|
||||
pinocchioInterface, collisionLinkPairs, collisionObjectPairs);
|
||||
if (verbose) {
|
||||
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::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});
|
||||
|
||||
|
|
|
@ -13,6 +13,9 @@ controller_manager:
|
|||
unitree_guide_controller:
|
||||
type: unitree_guide_controller/UnitreeGuideController
|
||||
|
||||
ocs2_quadruped_controller:
|
||||
type: ocs2_quadruped_controller/Ocs2QuadrupedController
|
||||
|
||||
imu_sensor_broadcaster:
|
||||
ros__parameters:
|
||||
sensor_name: "imu_sensor"
|
||||
|
@ -67,3 +70,53 @@ unitree_guide_controller:
|
|||
- linear_acceleration.x
|
||||
- linear_acceleration.y
|
||||
- linear_acceleration.z
|
||||
|
||||
ocs2_quadruped_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
|
|
@ -4,7 +4,7 @@ project(go2_description)
|
|||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY meshes xacro launch config
|
||||
DIRECTORY meshes xacro launch config urdf
|
||||
DESTINATION share/${PROJECT_NAME}/
|
||||
)
|
||||
|
||||
|
|
|
@ -20,11 +20,17 @@ source ~/ros2_ws/install/setup.bash
|
|||
ros2 launch go2_description visualize.launch.py
|
||||
```
|
||||
|
||||
## Launch Hardware Interface
|
||||
```bash
|
||||
source ~/ros2_ws/install/setup.bash
|
||||
ros2 launch go2_description hardware.launch.py
|
||||
```
|
||||
## Launch ROS2 Control
|
||||
* Unitree Guide Controller
|
||||
```bash
|
||||
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
|
||||
|
||||
|
|
|
@ -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
|
||||
}
|
||||
}
|
|
@ -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
|
||||
}
|
|
@ -13,6 +13,9 @@ controller_manager:
|
|||
unitree_guide_controller:
|
||||
type: unitree_guide_controller/UnitreeGuideController
|
||||
|
||||
ocs2_quadruped_controller:
|
||||
type: ocs2_quadruped_controller/Ocs2QuadrupedController
|
||||
|
||||
imu_sensor_broadcaster:
|
||||
ros__parameters:
|
||||
sensor_name: "imu_sensor"
|
||||
|
@ -67,3 +70,66 @@ unitree_guide_controller:
|
|||
- linear_acceleration.x
|
||||
- linear_acceleration.y
|
||||
- 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
|
|
@ -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]
|
||||
)
|
||||
])
|
|
@ -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>
|
Loading…
Reference in New Issue