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

ad qpoases colcon

This commit is contained in:
Zhenbiao Huang 2024-09-28 23:05:13 +08:00
parent ee4118b4e5
commit 00b6dbaeeb
22 changed files with 2688 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitmodules vendored
View File

@ -1,3 +0,0 @@
[submodule "submodules/qpOASES"]
path = submodules/qpOASES
url = https://github.com/coin-or/qpOASES

6
controllers/ocs2_quadruped_controller/CMakeLists.txt
View File

@ -19,11 +19,11 @@ set(CONTROLLER_INCLUDE_DEPENDS
control_input_msgs
angles
nav_msgs
qpoases_colcon
)
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(qpOASES REQUIRED)
foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED)
endforeach ()
@ -64,10 +64,6 @@ target_include_directories(${PROJECT_NAME}
ament_target_dependencies(${PROJECT_NAME}
${CONTROLLER_INCLUDE_DEPENDS}
)
target_link_libraries(${PROJECT_NAME}
${qpOASES_LIBRARY_DIR}/libqpOASES.a
)
pluginlib_export_plugin_description_file(controller_interface ocs2_quadruped_controller.xml)
install(

17
controllers/ocs2_quadruped_controller/README.md
View File

@ -12,15 +12,6 @@ and [ocs2_ros2](https://github.com/legubiao/ocs2_ros2).
colcon build --packages-up-to ocs2_legged_robot_ros
colcon build --packages-up-to ocs2_self_collision
```
* qpOASES
```bash
cd ~/ros2_ws/src/quadruped_ros2
git submodule update --init --recursive
```
also add below line to the Cmakelist.txt of qpOASES
```cmake
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC")
```
### 2.2 Build OCS2 Quadruped Controller
@ -30,8 +21,12 @@ colcon build --packages-up-to ocs2_quadruped_controller
```
## 3. Launch
* Go2 Robot
* Unitree Go1 Robot
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch go1_description ocs2_control.launch.py
```
* Unitree Go2 Robot
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch go2_description ocs2_control.launch.py

18
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WbcBase.h
View File

@ -33,7 +33,7 @@ namespace ocs2::legged_robot {
void updateDesired(const vector_t &stateDesired, const vector_t &inputDesired);
size_t getNumDecisionVars() const { return numDecisionVars_; }
size_t getNumDecisionVars() const { return num_decision_vars_; }
Task formulateFloatingBaseEomTask();
@ -49,20 +49,20 @@ namespace ocs2::legged_robot {
Task formulateContactForceTask(const vector_t &inputDesired) const;
size_t numDecisionVars_;
PinocchioInterface pinocchioInterfaceMeasured_, pinocchioInterfaceDesired_;
size_t num_decision_vars_;
PinocchioInterface pinocchio_interface_measured_, pinocchio_interface_desired_;
CentroidalModelInfo info_;
std::unique_ptr<PinocchioEndEffectorKinematics> eeKinematics_;
std::unique_ptr<PinocchioEndEffectorKinematics> ee_kinematics_;
CentroidalModelPinocchioMapping mapping_;
vector_t qMeasured_, vMeasured_, inputLast_;
vector_t q_measured_, v_measured_, input_last_;
matrix_t j_, dj_;
contact_flag_t contactFlag_{};
size_t numContacts_{};
contact_flag_t contact_flag_{};
size_t num_contacts_{};
// Task Parameters:
vector_t torqueLimits_;
scalar_t frictionCoeff_{}, swingKp_{}, swingKd_{};
vector_t torque_limits_;
scalar_t friction_coeff_{}, swing_kp_{}, swing_kd_{};
};
} // namespace legged

2
controllers/ocs2_quadruped_controller/package.xml
View File

@ -13,7 +13,7 @@
<depend>controller_interface</depend>
<depend>pluginlib</depend>
<depend>control_input_msgs</depend>
<depend>qpOASES</depend>
<depend>qpoases_colcon</depend>
<depend>ocs2_self_collision</depend>
<depend>angles</depend>

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

@ -86,6 +86,7 @@ namespace ocs2::legged_robot {
// Safety check, if failed, stop the controller
if (!safety_checker_->check(current_observation_, optimized_state, optimized_input)) {
RCLCPP_ERROR(get_node()->get_logger(), "[Legged Controller] Safety check failed, stopping the controller.");
return controller_interface::return_type::ERROR;
}
for (int i = 0; i < joint_names_.size(); i++) {

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

@ -70,7 +70,7 @@ namespace ocs2::legged_robot {
// Todo : load settings from ros param
model_settings_.jointNames = {
"FF_hip_joint", "FL_thigh_joint", "FL_calf_joint",
"FL_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"
@ -160,7 +160,7 @@ namespace ocs2::legged_robot {
std::make_unique<PinocchioInterface>(
centroidal_model::createPinocchioInterface(urdf_file, model_settings_.jointNames));
// CentroidalModelInfo
// CentroidModelInfo
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),

140
controllers/ocs2_quadruped_controller/src/wbc/WbcBase.cpp
View File

@ -15,25 +15,25 @@
namespace ocs2::legged_robot {
WbcBase::WbcBase(const PinocchioInterface &pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &eeKinematics)
: pinocchioInterfaceMeasured_(pinocchioInterface),
pinocchioInterfaceDesired_(pinocchioInterface),
: pinocchio_interface_measured_(pinocchioInterface),
pinocchio_interface_desired_(pinocchioInterface),
info_(std::move(info)),
ee_kinematics_(eeKinematics.clone()),
mapping_(info_),
inputLast_(vector_t::Zero(info_.inputDim)),
eeKinematics_(eeKinematics.clone()) {
numDecisionVars_ = info_.generalizedCoordinatesNum + 3 * info_.numThreeDofContacts + info_.actuatedDofNum;
qMeasured_ = vector_t(info_.generalizedCoordinatesNum);
vMeasured_ = vector_t(info_.generalizedCoordinatesNum);
input_last_(vector_t::Zero(info_.inputDim)) {
num_decision_vars_ = info_.generalizedCoordinatesNum + 3 * info_.numThreeDofContacts + info_.actuatedDofNum;
q_measured_ = vector_t(info_.generalizedCoordinatesNum);
v_measured_ = vector_t(info_.generalizedCoordinatesNum);
}
vector_t WbcBase::update(const vector_t &stateDesired, const vector_t &inputDesired,
const vector_t &rbdStateMeasured, size_t mode,
scalar_t /*period*/) {
contactFlag_ = modeNumber2StanceLeg(mode);
numContacts_ = 0;
for (bool flag: contactFlag_) {
contact_flag_ = modeNumber2StanceLeg(mode);
num_contacts_ = 0;
for (bool flag: contact_flag_) {
if (flag) {
numContacts_++;
num_contacts_++;
}
}
@ -44,25 +44,25 @@ namespace ocs2::legged_robot {
}
void WbcBase::updateMeasured(const vector_t &rbdStateMeasured) {
qMeasured_.head<3>() = rbdStateMeasured.segment<3>(3);
qMeasured_.segment<3>(3) = rbdStateMeasured.head<3>();
qMeasured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment(6, info_.actuatedDofNum);
vMeasured_.head<3>() = rbdStateMeasured.segment<3>(info_.generalizedCoordinatesNum + 3);
vMeasured_.segment<3>(3) = getEulerAnglesZyxDerivativesFromGlobalAngularVelocity<scalar_t>(
qMeasured_.segment<3>(3), rbdStateMeasured.segment<3>(info_.generalizedCoordinatesNum));
vMeasured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment(
q_measured_.head<3>() = rbdStateMeasured.segment<3>(3);
q_measured_.segment<3>(3) = rbdStateMeasured.head<3>();
q_measured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment(6, info_.actuatedDofNum);
v_measured_.head<3>() = rbdStateMeasured.segment<3>(info_.generalizedCoordinatesNum + 3);
v_measured_.segment<3>(3) = getEulerAnglesZyxDerivativesFromGlobalAngularVelocity<scalar_t>(
q_measured_.segment<3>(3), rbdStateMeasured.segment<3>(info_.generalizedCoordinatesNum));
v_measured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment(
info_.generalizedCoordinatesNum + 6, info_.actuatedDofNum);
const auto &model = pinocchioInterfaceMeasured_.getModel();
auto &data = pinocchioInterfaceMeasured_.getData();
const auto &model = pinocchio_interface_measured_.getModel();
auto &data = pinocchio_interface_measured_.getData();
// For floating base EoM task
forwardKinematics(model, data, qMeasured_, vMeasured_);
forwardKinematics(model, data, q_measured_, v_measured_);
computeJointJacobians(model, data);
updateFramePlacements(model, data);
crba(model, data, qMeasured_);
crba(model, data, q_measured_);
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
nonLinearEffects(model, data, qMeasured_, vMeasured_);
nonLinearEffects(model, data, q_measured_, v_measured_);
j_ = matrix_t(3 * info_.numThreeDofContacts, info_.generalizedCoordinatesNum);
for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
Eigen::Matrix<scalar_t, 6, Eigen::Dynamic> jac;
@ -73,7 +73,7 @@ namespace ocs2::legged_robot {
}
// For not contact motion task
computeJointJacobiansTimeVariation(model, data, qMeasured_, vMeasured_);
computeJointJacobiansTimeVariation(model, data, q_measured_, v_measured_);
dj_ = matrix_t(3 * info_.numThreeDofContacts, info_.generalizedCoordinatesNum);
for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
Eigen::Matrix<scalar_t, 6, Eigen::Dynamic> jac;
@ -85,27 +85,27 @@ namespace ocs2::legged_robot {
}
void WbcBase::updateDesired(const vector_t &stateDesired, const vector_t &inputDesired) {
const auto &model = pinocchioInterfaceDesired_.getModel();
auto &data = pinocchioInterfaceDesired_.getData();
const auto &model = pinocchio_interface_desired_.getModel();
auto &data = pinocchio_interface_desired_.getData();
mapping_.setPinocchioInterface(pinocchioInterfaceDesired_);
mapping_.setPinocchioInterface(pinocchio_interface_desired_);
const auto qDesired = mapping_.getPinocchioJointPosition(stateDesired);
forwardKinematics(model, data, qDesired);
computeJointJacobians(model, data, qDesired);
updateFramePlacements(model, data);
updateCentroidalDynamics(pinocchioInterfaceDesired_, info_, qDesired);
updateCentroidalDynamics(pinocchio_interface_desired_, info_, qDesired);
const vector_t vDesired = mapping_.getPinocchioJointVelocity(stateDesired, inputDesired);
forwardKinematics(model, data, qDesired, vDesired);
}
Task WbcBase::formulateFloatingBaseEomTask() {
auto &data = pinocchioInterfaceMeasured_.getData();
auto &data = pinocchio_interface_measured_.getData();
matrix_t s(info_.actuatedDofNum, info_.generalizedCoordinatesNum);
s.block(0, 0, info_.actuatedDofNum, 6).setZero();
s.block(0, 6, info_.actuatedDofNum, info_.actuatedDofNum).setIdentity();
matrix_t a = (matrix_t(info_.generalizedCoordinatesNum, numDecisionVars_) << data.M, -j_.transpose(), -s.
matrix_t a = (matrix_t(info_.generalizedCoordinatesNum, num_decision_vars_) << data.M, -j_.transpose(), -s.
transpose()).finished();
vector_t b = -data.nle;
@ -113,7 +113,7 @@ namespace ocs2::legged_robot {
}
Task WbcBase::formulateTorqueLimitsTask() {
matrix_t d(2 * info_.actuatedDofNum, numDecisionVars_);
matrix_t d(2 * info_.actuatedDofNum, num_decision_vars_);
d.setZero();
matrix_t i = matrix_t::Identity(info_.actuatedDofNum, info_.actuatedDofNum);
d.block(0, info_.generalizedCoordinatesNum + 3 * info_.numThreeDofContacts, info_.actuatedDofNum,
@ -123,23 +123,23 @@ namespace ocs2::legged_robot {
info_.actuatedDofNum) = -i;
vector_t f(2 * info_.actuatedDofNum);
for (size_t l = 0; l < 2 * info_.actuatedDofNum / 3; ++l) {
f.segment<3>(3 * l) = torqueLimits_;
f.segment<3>(3 * l) = torque_limits_;
}
return {matrix_t(), vector_t(), d, f};
}
Task WbcBase::formulateNoContactMotionTask() {
matrix_t a(3 * numContacts_, numDecisionVars_);
matrix_t a(3 * num_contacts_, num_decision_vars_);
vector_t b(a.rows());
a.setZero();
b.setZero();
size_t j = 0;
for (size_t i = 0; i < info_.numThreeDofContacts; i++) {
if (contactFlag_[i]) {
if (contact_flag_[i]) {
a.block(3 * j, 0, 3, info_.generalizedCoordinatesNum) = j_.block(
3 * i, 0, 3, info_.generalizedCoordinatesNum);
b.segment(3 * j, 3) = -dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) * vMeasured_;
b.segment(3 * j, 3) = -dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) * v_measured_;
j++;
}
}
@ -148,11 +148,11 @@ namespace ocs2::legged_robot {
}
Task WbcBase::formulateFrictionConeTask() {
matrix_t a(3 * (info_.numThreeDofContacts - numContacts_), numDecisionVars_);
matrix_t a(3 * (info_.numThreeDofContacts - num_contacts_), num_decision_vars_);
a.setZero();
size_t j = 0;
for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
if (!contactFlag_[i]) {
if (!contact_flag_[i]) {
a.block(3 * j++, info_.generalizedCoordinatesNum + 3 * i, 3, 3) = matrix_t::Identity(3, 3);
}
}
@ -161,16 +161,16 @@ namespace ocs2::legged_robot {
matrix_t frictionPyramic(5, 3); // clang-format off
frictionPyramic << 0, 0, -1,
1, 0, -frictionCoeff_,
-1, 0, -frictionCoeff_,
0, 1, -frictionCoeff_,
0, -1, -frictionCoeff_; // clang-format on
1, 0, -friction_coeff_,
-1, 0, -friction_coeff_,
0, 1, -friction_coeff_,
0, -1, -friction_coeff_; // clang-format on
matrix_t d(5 * numContacts_ + 3 * (info_.numThreeDofContacts - numContacts_), numDecisionVars_);
matrix_t d(5 * num_contacts_ + 3 * (info_.numThreeDofContacts - num_contacts_), num_decision_vars_);
d.setZero();
j = 0;
for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
if (contactFlag_[i]) {
if (contact_flag_[i]) {
d.block(5 * j++, info_.generalizedCoordinatesNum + 3 * i, 5, 3) = frictionPyramic;
}
}
@ -180,26 +180,26 @@ namespace ocs2::legged_robot {
}
Task WbcBase::formulateBaseAccelTask(const vector_t &stateDesired, const vector_t &inputDesired, scalar_t period) {
matrix_t a(6, numDecisionVars_);
matrix_t a(6, num_decision_vars_);
a.setZero();
a.block(0, 0, 6, 6) = matrix_t::Identity(6, 6);
vector_t jointAccel = centroidal_model::getJointVelocities(inputDesired - inputLast_, info_) / period;
inputLast_ = inputDesired;
mapping_.setPinocchioInterface(pinocchioInterfaceDesired_);
vector_t jointAccel = centroidal_model::getJointVelocities(inputDesired - input_last_, info_) / period;
input_last_ = inputDesired;
mapping_.setPinocchioInterface(pinocchio_interface_desired_);
const auto &model = pinocchioInterfaceDesired_.getModel();
auto &data = pinocchioInterfaceDesired_.getData();
const auto &model = pinocchio_interface_desired_.getModel();
auto &data = pinocchio_interface_desired_.getData();
const auto qDesired = mapping_.getPinocchioJointPosition(stateDesired);
const vector_t vDesired = mapping_.getPinocchioJointVelocity(stateDesired, inputDesired);
const auto &A = getCentroidalMomentumMatrix(pinocchioInterfaceDesired_);
const auto &A = getCentroidalMomentumMatrix(pinocchio_interface_desired_);
const Matrix6 Ab = A.template leftCols<6>();
const auto AbInv = computeFloatingBaseCentroidalMomentumMatrixInverse(Ab);
const auto Aj = A.rightCols(info_.actuatedDofNum);
const auto ADot = dccrba(model, data, qDesired, vDesired);
Vector6 centroidalMomentumRate = info_.robotMass * getNormalizedCentroidalMomentumRate(
pinocchioInterfaceDesired_, info_, inputDesired);
pinocchio_interface_desired_, info_, inputDesired);
centroidalMomentumRate.noalias() -= ADot * vDesired;
centroidalMomentumRate.noalias() -= Aj * jointAccel;
@ -209,25 +209,25 @@ namespace ocs2::legged_robot {
}
Task WbcBase::formulateSwingLegTask() {
eeKinematics_->setPinocchioInterface(pinocchioInterfaceMeasured_);
std::vector<vector3_t> posMeasured = eeKinematics_->getPosition(vector_t());
std::vector<vector3_t> velMeasured = eeKinematics_->getVelocity(vector_t(), vector_t());
eeKinematics_->setPinocchioInterface(pinocchioInterfaceDesired_);
std::vector<vector3_t> posDesired = eeKinematics_->getPosition(vector_t());
std::vector<vector3_t> velDesired = eeKinematics_->getVelocity(vector_t(), vector_t());
ee_kinematics_->setPinocchioInterface(pinocchio_interface_measured_);
std::vector<vector3_t> posMeasured = ee_kinematics_->getPosition(vector_t());
std::vector<vector3_t> velMeasured = ee_kinematics_->getVelocity(vector_t(), vector_t());
ee_kinematics_->setPinocchioInterface(pinocchio_interface_desired_);
std::vector<vector3_t> posDesired = ee_kinematics_->getPosition(vector_t());
std::vector<vector3_t> velDesired = ee_kinematics_->getVelocity(vector_t(), vector_t());
matrix_t a(3 * (info_.numThreeDofContacts - numContacts_), numDecisionVars_);
matrix_t a(3 * (info_.numThreeDofContacts - num_contacts_), num_decision_vars_);
vector_t b(a.rows());
a.setZero();
b.setZero();
size_t j = 0;
for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
if (!contactFlag_[i]) {
vector3_t accel = swingKp_ * (posDesired[i] - posMeasured[i]) + swingKd_ * (
if (!contact_flag_[i]) {
vector3_t accel = swing_kp_ * (posDesired[i] - posMeasured[i]) + swing_kd_ * (
velDesired[i] - velMeasured[i]);
a.block(3 * j, 0, 3, info_.generalizedCoordinatesNum) = j_.block(
3 * i, 0, 3, info_.generalizedCoordinatesNum);
b.segment(3 * j, 3) = accel - dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) * vMeasured_;
b.segment(3 * j, 3) = accel - dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) * v_measured_;
j++;
}
}
@ -236,7 +236,7 @@ namespace ocs2::legged_robot {
}
Task WbcBase::formulateContactForceTask(const vector_t &inputDesired) const {
matrix_t a(3 * info_.numThreeDofContacts, numDecisionVars_);
matrix_t a(3 * info_.numThreeDofContacts, num_decision_vars_);
vector_t b(a.rows());
a.setZero();
@ -248,24 +248,24 @@ namespace ocs2::legged_robot {
return {a, b, matrix_t(), vector_t()};
}
void WbcBase::loadTasksSetting(const std::string &taskFile, bool verbose) {
void WbcBase::loadTasksSetting(const std::string &taskFile, const bool verbose) {
// Load task file
torqueLimits_ = vector_t(info_.actuatedDofNum / 4);
loadData::loadEigenMatrix(taskFile, "torqueLimitsTask", torqueLimits_);
torque_limits_ = vector_t(info_.actuatedDofNum / 4);
loadData::loadEigenMatrix(taskFile, "torqueLimitsTask", torque_limits_);
if (verbose) {
std::cerr << "\n #### Torque Limits Task:";
std::cerr << "\n #### =============================================================================\n";
std::cerr << "\n #### HAA HFE KFE: " << torqueLimits_.transpose() << "\n";
std::cerr << "\n #### Hip_joint Thigh_joint Calf_joint: " << torque_limits_.transpose() << "\n";
std::cerr << " #### =============================================================================\n";
}
boost::property_tree::ptree pt;
boost::property_tree::read_info(taskFile, pt);
read_info(taskFile, pt);
std::string prefix = "frictionConeTask.";
if (verbose) {
std::cerr << "\n #### Friction Cone Task:";
std::cerr << "\n #### =============================================================================\n";
}
loadData::loadPtreeValue(pt, frictionCoeff_, prefix + "frictionCoefficient", verbose);
loadData::loadPtreeValue(pt, friction_coeff_, prefix + "frictionCoefficient", verbose);
if (verbose) {
std::cerr << " #### =============================================================================\n";
}
@ -274,7 +274,7 @@ namespace ocs2::legged_robot {
std::cerr << "\n #### Swing Leg Task:";
std::cerr << "\n #### =============================================================================\n";
}
loadData::loadPtreeValue(pt, swingKp_, prefix + "kp", verbose);
loadData::loadPtreeValue(pt, swingKd_, prefix + "kd", verbose);
loadData::loadPtreeValue(pt, swing_kp_, prefix + "kp", verbose);
loadData::loadPtreeValue(pt, swing_kd_, prefix + "kd", verbose);
}
} // namespace legged

2
descriptions/go1_description/CMakeLists.txt
View File

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

16
descriptions/go1_description/README.md
View File

@ -14,8 +14,14 @@ source ~/ros2_ws/install/setup.bash
ros2 launch go1_description visualize.launch.py
```
## Launch Hardware Interface
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch go1_description hardware.launch.py
```
## Launch ROS2 Control
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch go1_description unitree_guide.launch.py
```
* OCS2 Quadruped Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch go1_description ocs2_control.launch.py
```

255
descriptions/go1_description/config/ocs2/gait.info Normal file
View File

@ -0,0 +1,255 @@
list
{
[0] stance
[1] trot
[2] standing_trot
[3] flying_trot
[4] pace
[5] standing_pace
[6] dynamic_walk
[7] static_walk
[8] amble
[9] lindyhop
[10] skipping
[11] pawup
}
stance
{
modeSequence
{
[0] STANCE
}
switchingTimes
{
[0] 0.0
[1] 0.5
}
}
trot
{
modeSequence
{
[0] LF_RH
[1] RF_LH
}
switchingTimes
{
[0] 0.0
[1] 0.3
[2] 0.6
}
}
standing_trot
{
modeSequence
{
[0] LF_RH
[1] STANCE
[2] RF_LH
[3] STANCE
}
switchingTimes
{
[0] 0.00
[1] 0.25
[2] 0.3
[3] 0.55
[4] 0.6
}
}
flying_trot
{
modeSequence
{
[0] LF_RH
[1] FLY
[2] RF_LH
[3] FLY
}
switchingTimes
{
[0] 0.00
[1] 0.15
[2] 0.2
[3] 0.35
[4] 0.4
}
}
pace
{
modeSequence
{
[0] LF_LH
[1] FLY
[2] RF_RH
[3] FLY
}
switchingTimes
{
[0] 0.0
[1] 0.28
[2] 0.30
[3] 0.58
[4] 0.60
}
}
standing_pace
{
modeSequence
{
[0] LF_LH
[1] STANCE
[2] RF_RH
[3] STANCE
}
switchingTimes
{
[0] 0.0
[1] 0.30
[2] 0.35
[3] 0.65
[4] 0.70
}
}
dynamic_walk
{
modeSequence
{
[0] LF_RF_RH
[1] RF_RH
[2] RF_LH_RH
[3] LF_RF_LH
[4] LF_LH
[5] LF_LH_RH
}
switchingTimes
{
[0] 0.0
[1] 0.2
[2] 0.3
[3] 0.5
[4] 0.7
[5] 0.8
[6] 1.0
}
}
static_walk
{
modeSequence
{
[0] LF_RF_RH
[1] RF_LH_RH
[2] LF_RF_LH
[3] LF_LH_RH
}
switchingTimes
{
[0] 0.0
[1] 0.3
[2] 0.6
[3] 0.9
[4] 1.2
}
}
amble
{
modeSequence
{
[0] RF_LH
[1] LF_LH
[2] LF_RH
[3] RF_RH
}
switchingTimes
{
[0] 0.0
[1] 0.15
[2] 0.40
[3] 0.55
[4] 0.80
}
}
lindyhop
{
modeSequence
{
[0] LF_RH
[1] STANCE
[2] RF_LH
[3] STANCE
[4] LF_LH
[5] RF_RH
[6] LF_LH
[7] STANCE
[8] RF_RH
[9] LF_LH
[10] RF_RH
[11] STANCE
}
switchingTimes
{
[0] 0.00 ; Step 1
[1] 0.35 ; Stance
[2] 0.45 ; Step 2
[3] 0.80 ; Stance
[4] 0.90 ; Tripple step
[5] 1.125 ;
[6] 1.35 ;
[7] 1.70 ; Stance
[8] 1.80 ; Tripple step
[9] 2.025 ;
[10] 2.25 ;
[11] 2.60 ; Stance
[12] 2.70 ;
}
}
skipping
{
modeSequence
{
[0] LF_RH
[1] FLY
[2] LF_RH
[3] FLY
[4] RF_LH
[5] FLY
[6] RF_LH
[7] FLY
}
switchingTimes
{
[0] 0.00
[1] 0.27
[2] 0.30
[3] 0.57
[4] 0.60
[5] 0.87
[6] 0.90
[7] 1.17
[8] 1.20
}
}
pawup
{
modeSequence
{
[0] RF_LH_RH
}
switchingTimes
{
[0] 0.0
[1] 2.0
}
}

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

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

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

@ -0,0 +1,319 @@
centroidalModelType 0 // 0: FullCentroidalDynamics, 1: Single Rigid Body Dynamics
legged_robot_interface
{
verbose false // show the loaded parameters
}
model_settings
{
positionErrorGain 0.0
phaseTransitionStanceTime 0.1
verboseCppAd true
recompileLibrariesCppAd false
modelFolderCppAd /tmp/ocs2_quadruped_controller/go1
}
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
}

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

@ -74,19 +74,20 @@ unitree_guide_controller:
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
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
command_interfaces:
- effort
@ -101,10 +102,11 @@ ocs2_quadruped_controller:
- velocity
feet_names:
- FR_foot
- FL_foot
- RR_foot
- RL_foot
- FR_foot
- RR_foot
imu_name: "imu_sensor"
base_name: "base"
@ -119,4 +121,11 @@ ocs2_quadruped_controller:
- angular_velocity.z
- linear_acceleration.x
- linear_acceleration.y
- linear_acceleration.z
- linear_acceleration.z
foot_force_name: "foot_force"
foot_force_interfaces:
- FL
- RL
- FR
- RR

122
descriptions/go1_description/launch/ocs2_control.launch.py Normal file
View File

@ -0,0 +1,122 @@
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 = "go1_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'),
'gait_file': os.path.join(get_package_share_directory(package_description), 'config',
'ocs2', 'gait.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
descriptions/go1_description/launch/hardware.launch.py → descriptions/go1_description/launch/unitree_guide.launch.py
View File

1549
descriptions/go1_description/urdf/robot.urdf Normal file
View File

File diff suppressed because it is too large Load Diff

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

@ -75,10 +75,6 @@ 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

60
libraries/qpoases_colcon/CMakeLists.txt Normal file
View File

@ -0,0 +1,60 @@
cmake_minimum_required(VERSION 3.8)
project(qpoases_colcon)
set(CMAKE_BUILD_TYPE Release)
# find dependencies
find_package(ament_cmake REQUIRED)
include(FetchContent)
# Define directories
set(QPOASES_DEVEL_PREFIX ${CMAKE_INSTALL_PREFIX} CACHE STRING "QPOASES install path")
set(QPOASES_INCLUDE_DIR ${QPOASES_DEVEL_PREFIX}/include)
set(QPOASES_LIB_DIR ${QPOASES_DEVEL_PREFIX}/lib)
set(QPOASES_DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/download)
set(QPOASES_BUILD_DIR ${CMAKE_CURRENT_BINARY_DIR}/build)
# Create directories if they do not exist
file(MAKE_DIRECTORY ${QPOASES_INCLUDE_DIR})
file(MAKE_DIRECTORY ${QPOASES_LIB_DIR})
file(MAKE_DIRECTORY ${QPOASES_DOWNLOAD_DIR})
file(MAKE_DIRECTORY ${QPOASES_BUILD_DIR})
# QPOASES Settings
set(BUILD_SHARED_LIBS ON CACHE STRING "Build shared libraries" FORCE)
set(QPOASES_BUILD_EXAMPLES OFF CACHE BOOL "Examples disable")
# Download & build source
FetchContent_Declare(qpoasesDownload
GIT_REPOSITORY https://github.com/coin-or/qpOASES
UPDATE_COMMAND ""
SOURCE_DIR ${QPOASES_DOWNLOAD_DIR}
BINARY_DIR ${QPOASES_BUILD_DIR}
BUILD_COMMAND $(MAKE)
INSTALL_COMMAND "$(MAKE) install"
)
FetchContent_MakeAvailable(qpoasesDownload)
# Copy header to where ament_cmake expects them
file(COPY ${QPOASES_DOWNLOAD_DIR}/include/qpOASES.hpp DESTINATION ${QPOASES_INCLUDE_DIR})
file(GLOB_RECURSE HEADERS "${QPOASES_DOWNLOAD_DIR}/include/qpOASES/*")
foreach (HEADER_FILE ${HEADERS})
message(STATUS "FOUND HEADER: " ${HEADER_FILE})
file(COPY ${HEADER_FILE} DESTINATION ${QPOASES_INCLUDE_DIR}/qpOASES)
endforeach ()
file(GLOB_RECURSE HEADERS "${QPOASES_DOWNLOAD_DIR}/include/qpOASES/extras/*")
foreach (HEADER_FILE ${HEADERS})
message(STATUS "FOUND HEADER: " ${HEADER_FILE})
file(COPY ${HEADER_FILE} DESTINATION ${QPOASES_INCLUDE_DIR}/qpOASES/extras)
endforeach ()
# Propagate dependencies
ament_export_include_directories(${QPOASES_INCLUDE_DIR})
ament_export_libraries(qpOASES)
install(TARGETS qpOASES
LIBRARY DESTINATION lib)
ament_package()

202
libraries/qpoases_colcon/LICENSE Normal file
View File

@ -0,0 +1,202 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

15
libraries/qpoases_colcon/package.xml Normal file
View File

@ -0,0 +1,15 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>qpoases_colcon</name>
<version>0.0.0</version>
<description>Colcon wrapper for qpOASES</description>
<maintainer email="biao876990970@hotmail.com">biao</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

1
submodules/qpOASES

@ -1 +0,0 @@
Subproject commit 0b86dbf00c7fce34420bedc5914f71b176fe79d3