fixed the frequency problem
This commit is contained in:
Zhenbiao Huang
parent
691734dc19
commit
9646e428c6
|
|
@ -27,10 +27,8 @@ void KeyboardInput::timer_callback() {
|
|||
inputs_.rx = 0;
|
||||
inputs_.ry = 0;
|
||||
}
|
||||
} else {
|
||||
inputs_.command = 0;
|
||||
publisher_->publish(inputs_);
|
||||
}
|
||||
publisher_->publish(inputs_);
|
||||
}
|
||||
|
||||
void KeyboardInput::check_command(const char key) {
|
||||
|
|
|
|||
|
|
@ -24,14 +24,23 @@ foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS})
|
|||
endforeach ()
|
||||
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
|
||||
src/UnitreeGuideController.cpp
|
||||
|
||||
src/FSM/StatePassive.cpp
|
||||
src/FSM/StateFixedDown.cpp
|
||||
src/FSM/StateFixedStand.cpp
|
||||
src/FSM/StateSwingTest.cpp
|
||||
src/FSM/StateFreeStand.cpp
|
||||
src/robotics/QuadrupedRobot.cpp
|
||||
src/robotics/RobotLeg.cpp
|
||||
|
||||
src/robot/QuadrupedRobot.cpp
|
||||
src/robot/RobotLeg.cpp
|
||||
|
||||
src/control/Estimator.cpp
|
||||
|
||||
src/quadProgpp/Array.cc
|
||||
src/quadProgpp/QuadProg++.cc
|
||||
|
||||
)
|
||||
target_include_directories(${PROJECT_NAME}
|
||||
PUBLIC
|
||||
|
|
|
|||
|
|
@ -1,7 +1,6 @@
|
|||
unitree_guide_controller:
|
||||
ros__parameters:
|
||||
type: unitree_guide_controller/UnitreeGuideController
|
||||
update_rate: 200 # Hz
|
||||
joints:
|
||||
- FR_hip_joint
|
||||
- FR_thigh_joint
|
||||
|
|
|
|||
|
|
@ -9,7 +9,7 @@
|
|||
#include <utility>
|
||||
#include <rclcpp/time.hpp>
|
||||
#include <unitree_guide_controller/common/enumClass.h>
|
||||
#include <unitree_guide_controller/common/interface.h>
|
||||
#include <unitree_guide_controller/control/CtrlComponent.h>
|
||||
|
||||
class FSMState {
|
||||
public:
|
||||
|
|
|
|||
|
|
@ -28,7 +28,7 @@ private:
|
|||
double start_pos_[12] = {};
|
||||
rclcpp::Time start_time_;
|
||||
|
||||
double duration_ = 6000; // steps
|
||||
double duration_ = 600; // steps
|
||||
double percent_ = 0; //%
|
||||
double phase = 0.0;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ private:
|
|||
double start_pos_[12] = {};
|
||||
rclcpp::Time start_time_;
|
||||
|
||||
double duration_ = 6000; // steps
|
||||
double duration_ = 600; // steps
|
||||
double percent_ = 0; //%
|
||||
double phase = 0.0;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -96,6 +96,9 @@ namespace unitree_guide_controller {
|
|||
std::shared_ptr<FSMState> current_state_;
|
||||
std::shared_ptr<FSMState> next_state_;
|
||||
|
||||
std::chrono::time_point<std::chrono::steady_clock> last_update_time_;
|
||||
double update_frequency_;
|
||||
|
||||
std::shared_ptr<FSMState> getNextState(FSMStateName stateName) const;
|
||||
|
||||
std::unordered_map<
|
||||
|
|
|
|||
|
|
@ -9,7 +9,7 @@
|
|||
#include <hardware_interface/loaned_command_interface.hpp>
|
||||
#include <hardware_interface/loaned_state_interface.hpp>
|
||||
#include <control_input_msgs/msg/inputs.hpp>
|
||||
#include <unitree_guide_controller/robotics/QuadrupedRobot.h>
|
||||
#include <unitree_guide_controller/robot/QuadrupedRobot.h>
|
||||
|
||||
struct CtrlComponent {
|
||||
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
|
||||
|
|
@ -38,7 +38,7 @@ struct CtrlComponent {
|
|||
QuadrupedRobot default_robot_model_;
|
||||
std::reference_wrapper<QuadrupedRobot> robot_model_;
|
||||
|
||||
CtrlComponent() : control_inputs_(default_inputs_), default_robot_model_(), robot_model_(default_robot_model_) {
|
||||
CtrlComponent() : control_inputs_(default_inputs_), robot_model_(default_robot_model_) {
|
||||
}
|
||||
|
||||
void clear() {
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
//
|
||||
// Created by biao on 24-9-14.
|
||||
//
|
||||
|
||||
#ifndef ESTIMATOR_H
|
||||
#define ESTIMATOR_H
|
||||
|
||||
|
||||
|
||||
class Estimator {
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //ESTIMATOR_H
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,78 @@
|
|||
/*
|
||||
File $Id: QuadProg++.hh 232 2007-06-21 12:29:00Z digasper $
|
||||
|
||||
The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
|
||||
for the solution of a (convex) Quadratic Programming problem
|
||||
by means of an active-set dual method.
|
||||
|
||||
The problem is in the form:
|
||||
|
||||
min 0.5 * x G x + g0 x
|
||||
s.t.
|
||||
CE^T x + ce0 = 0
|
||||
CI^T x + ci0 >= 0
|
||||
|
||||
The matrix and vectors dimensions are as follows:
|
||||
G: n * n
|
||||
g0: n
|
||||
|
||||
CE: n * p
|
||||
ce0: p
|
||||
|
||||
CI: n * m
|
||||
ci0: m
|
||||
|
||||
x: n
|
||||
|
||||
The function will return the cost of the solution written in the x vector or
|
||||
std::numeric_limits::infinity() if the problem is infeasible. In the latter
|
||||
case the value of the x vector is not correct.
|
||||
|
||||
References: D. Goldfarb, A. Idnani. A numerically stable dual method for
|
||||
solving strictly convex quadratic programs. Mathematical Programming 27 (1983)
|
||||
pp. 1-33.
|
||||
|
||||
Notes:
|
||||
1. pay attention in setting up the vectors ce0 and ci0.
|
||||
If the constraints of your problem are specified in the form
|
||||
A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
|
||||
2. The matrices have column dimension equal to MATRIX_DIM,
|
||||
a constant set to 20 in this file (by means of a #define macro).
|
||||
If the matrices are bigger than 20 x 20 the limit could be
|
||||
increased by means of a -DMATRIX_DIM=n on the compiler command
|
||||
line.
|
||||
3. The matrix G is modified within the function since it is used to compute
|
||||
the G = L^T L cholesky factorization for further computations inside the
|
||||
function. If you need the original matrix G you should make a copy of it and
|
||||
pass the copy to the function.
|
||||
|
||||
Author: Luca Di Gaspero
|
||||
DIEGM - University of Udine, Italy
|
||||
luca.digaspero@uniud.it
|
||||
http://www.diegm.uniud.it/digaspero/
|
||||
|
||||
The author will be grateful if the researchers using this software will
|
||||
acknowledge the contribution of this function in their research papers.
|
||||
|
||||
Copyright (c) 2007-2016 Luca Di Gaspero
|
||||
|
||||
This software may be modified and distributed under the terms
|
||||
of the MIT license. See the LICENSE file for details.
|
||||
*/
|
||||
|
||||
#ifndef _QUADPROGPP
|
||||
#define _QUADPROGPP
|
||||
|
||||
#include "Array.hh"
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
namespace quadprogpp {
|
||||
|
||||
double solve_quadprog(Matrix<double>& G, Vector<double>& g0,
|
||||
const Matrix<double>& CE, const Vector<double>& ce0,
|
||||
const Matrix<double>& CI, const Vector<double>& ci0,
|
||||
Vector<double>& x);
|
||||
|
||||
} // namespace quadprogpp
|
||||
|
||||
#endif // #define _QUADPROGPP
|
||||
|
|
@ -2,12 +2,13 @@
|
|||
// Created by tlab-uav on 24-9-11.
|
||||
//
|
||||
|
||||
#include "unitree_guide_controller/FSM/StateFixedDown.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <unitree_guide_controller/FSM/StateFixedDown.h>
|
||||
|
||||
StateFixedDown::StateFixedDown(CtrlComponent ctrlComp): FSMState(
|
||||
FSMStateName::FIXEDDOWN, "fixed down", std::move(ctrlComp)) {
|
||||
// duration_ = ctrlComp_.frequency_ * 15;
|
||||
duration_ = ctrlComp_.frequency_ * 1.2;
|
||||
}
|
||||
|
||||
void StateFixedDown::enter() {
|
||||
|
|
@ -34,7 +35,7 @@ void StateFixedDown::exit() {
|
|||
}
|
||||
|
||||
FSMStateName StateFixedDown::checkChange() {
|
||||
if (percent_ < 2) {
|
||||
if (percent_ < 1.5) {
|
||||
return FSMStateName::FIXEDDOWN;
|
||||
}
|
||||
switch (ctrlComp_.control_inputs_.get().command) {
|
||||
|
|
|
|||
|
|
@ -2,12 +2,13 @@
|
|||
// Created by biao on 24-9-10.
|
||||
//
|
||||
|
||||
#include "unitree_guide_controller/FSM/StateFixedStand.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <unitree_guide_controller/FSM/StateFixedStand.h>
|
||||
|
||||
StateFixedStand::StateFixedStand(CtrlComponent ctrlComp): FSMState(
|
||||
FSMStateName::FIXEDSTAND, "fixed stand", std::move(ctrlComp)) {
|
||||
duration_ = ctrlComp_.frequency_ * 1.2;
|
||||
}
|
||||
|
||||
void StateFixedStand::enter() {
|
||||
|
|
@ -35,7 +36,7 @@ void StateFixedStand::exit() {
|
|||
}
|
||||
|
||||
FSMStateName StateFixedStand::checkChange() {
|
||||
if (percent_ < 2) {
|
||||
if (percent_ < 1.5) {
|
||||
return FSMStateName::FIXEDSTAND;
|
||||
}
|
||||
switch (ctrlComp_.control_inputs_.get().command) {
|
||||
|
|
|
|||
|
|
@ -2,8 +2,8 @@
|
|||
// Created by tlab-uav on 24-9-13.
|
||||
//
|
||||
|
||||
#include <unitree_guide_controller/common/mathTools.h>
|
||||
#include <unitree_guide_controller/FSM/StateFreeStand.h>
|
||||
#include "unitree_guide_controller/FSM/StateFreeStand.h"
|
||||
#include "unitree_guide_controller/common/mathTools.h"
|
||||
|
||||
StateFreeStand::StateFreeStand(CtrlComponent ctrl_component) : FSMState(FSMStateName::FREESTAND, "free stand",
|
||||
std::move(ctrl_component)) {
|
||||
|
|
|
|||
|
|
@ -2,9 +2,9 @@
|
|||
// Created by tlab-uav on 24-9-6.
|
||||
//
|
||||
|
||||
#include <iostream>
|
||||
#include <unitree_guide_controller/FSM/StatePassive.h>
|
||||
#include "unitree_guide_controller/FSM/StatePassive.h"
|
||||
|
||||
#include <iostream>
|
||||
#include <utility>
|
||||
|
||||
StatePassive::StatePassive(CtrlComponent ctrlComp) : FSMState(
|
||||
|
|
|
|||
|
|
@ -3,8 +3,8 @@
|
|||
//
|
||||
|
||||
#include <iostream>
|
||||
#include <unitree_guide_controller/FSM/StateSwingTest.h>
|
||||
#include <unitree_guide_controller/common/mathTools.h>
|
||||
#include "unitree_guide_controller/FSM/StateSwingTest.h"
|
||||
#include "unitree_guide_controller/common/mathTools.h"
|
||||
|
||||
StateSwingTest::StateSwingTest(CtrlComponent ctrlComp): FSMState(
|
||||
FSMStateName::SWINGTEST, "swing test", std::move(ctrlComp)) {
|
||||
|
|
|
|||
|
|
@ -2,9 +2,9 @@
|
|||
// Created by tlab-uav on 24-9-6.
|
||||
//
|
||||
|
||||
#include <unitree_guide_controller/UnitreeGuideController.h>
|
||||
#include <unitree_guide_controller/FSM/StatePassive.h>
|
||||
#include <unitree_guide_controller/robotics/QuadrupedRobot.h>
|
||||
#include "unitree_guide_controller/UnitreeGuideController.h"
|
||||
#include "unitree_guide_controller/FSM/StatePassive.h"
|
||||
#include "unitree_guide_controller/robot/QuadrupedRobot.h"
|
||||
|
||||
namespace unitree_guide_controller {
|
||||
using config_type = controller_interface::interface_configuration_type;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,5 @@
|
|||
//
|
||||
// Created by biao on 24-9-14.
|
||||
//
|
||||
|
||||
#include "unitree_guide_controller/control/Estimator.h"
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
// $Id: Array.cc 201 2008-05-18 19:47:38Z digasper $
|
||||
// This file is part of QuadProg++:
|
||||
// Copyright (C) 2006--2009 Luca Di Gaspero.
|
||||
//
|
||||
// This software may be modified and distributed under the terms
|
||||
// of the MIT license. See the LICENSE file for details.
|
||||
|
||||
#include "unitree_guide_controller/quadProgpp/Array.hh"
|
||||
|
||||
/**
|
||||
Index utilities
|
||||
*/
|
||||
|
||||
namespace quadprogpp {
|
||||
std::set<unsigned int> seq(unsigned int s, unsigned int e) {
|
||||
std::set<unsigned int> tmp;
|
||||
for (unsigned int i = s; i <= e; i++) tmp.insert(i);
|
||||
|
||||
return tmp;
|
||||
}
|
||||
|
||||
std::set<unsigned int> singleton(unsigned int i) {
|
||||
std::set<unsigned int> tmp;
|
||||
tmp.insert(i);
|
||||
|
||||
return tmp;
|
||||
}
|
||||
} // namespace quadprogpp
|
||||
|
|
@ -0,0 +1,729 @@
|
|||
/*
|
||||
File $Id: QuadProg++.cc 232 2007-06-21 12:29:00Z digasper $
|
||||
|
||||
Author: Luca Di Gaspero
|
||||
DIEGM - University of Udine, Italy
|
||||
luca.digaspero@uniud.it
|
||||
http://www.diegm.uniud.it/digaspero/
|
||||
|
||||
This software may be modified and distributed under the terms
|
||||
of the MIT license. See the LICENSE file for details.
|
||||
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include "unitree_guide_controller/quadProgpp/QuadProg++.hh"
|
||||
// #define TRACE_SOLVER
|
||||
|
||||
namespace quadprogpp {
|
||||
// Utility functions for updating some data needed by the solution method
|
||||
void compute_d(Vector<double> &d, const Matrix<double> &J,
|
||||
const Vector<double> &np);
|
||||
|
||||
void update_z(Vector<double> &z, const Matrix<double> &J,
|
||||
const Vector<double> &d, int iq);
|
||||
|
||||
void update_r(const Matrix<double> &R, Vector<double> &r,
|
||||
const Vector<double> &d, int iq);
|
||||
|
||||
bool add_constraint(Matrix<double> &R, Matrix<double> &J, Vector<double> &d,
|
||||
unsigned int &iq, double &rnorm);
|
||||
|
||||
void delete_constraint(Matrix<double> &R, Matrix<double> &J, Vector<int> &A,
|
||||
Vector<double> &u, unsigned int n, int p,
|
||||
unsigned int &iq, int l);
|
||||
|
||||
// Utility functions for computing the Cholesky decomposition and solving
|
||||
// linear systems
|
||||
void cholesky_decomposition(Matrix<double> &A);
|
||||
|
||||
void cholesky_solve(const Matrix<double> &L, Vector<double> &x,
|
||||
const Vector<double> &b);
|
||||
|
||||
void forward_elimination(const Matrix<double> &L, Vector<double> &y,
|
||||
const Vector<double> &b);
|
||||
|
||||
void backward_elimination(const Matrix<double> &U, Vector<double> &x,
|
||||
const Vector<double> &y);
|
||||
|
||||
// Utility functions for computing the scalar product and the euclidean
|
||||
// distance between two numbers
|
||||
double scalar_product(const Vector<double> &x, const Vector<double> &y);
|
||||
|
||||
double distance(double a, double b);
|
||||
|
||||
// Utility functions for printing vectors and matrices
|
||||
void print_matrix(const char *name, const Matrix<double> &A, int n = -1,
|
||||
int m = -1);
|
||||
|
||||
template<typename T>
|
||||
void print_vector(const char *name, const Vector<T> &v, int n = -1);
|
||||
|
||||
// The Solving function, implementing the Goldfarb-Idnani method
|
||||
double solve_quadprog(Matrix<double> &G, Vector<double> &g0,
|
||||
const Matrix<double> &CE, const Vector<double> &ce0,
|
||||
const Matrix<double> &CI, const Vector<double> &ci0,
|
||||
Vector<double> &x) {
|
||||
std::ostringstream msg;
|
||||
unsigned int n = G.ncols(), p = CE.ncols(), m = CI.ncols();
|
||||
if (G.nrows() != n) {
|
||||
msg << "The matrix G is not a squared matrix (" << G.nrows() << " x "
|
||||
<< G.ncols() << ")";
|
||||
throw std::logic_error(msg.str());
|
||||
}
|
||||
if (CE.nrows() != n) {
|
||||
msg << "The matrix CE is incompatible (incorrect number of rows "
|
||||
<< CE.nrows() << " , expecting " << n << ")";
|
||||
throw std::logic_error(msg.str());
|
||||
}
|
||||
if (ce0.size() != p) {
|
||||
msg << "The vector ce0 is incompatible (incorrect dimension " << ce0.size()
|
||||
<< ", expecting " << p << ")";
|
||||
throw std::logic_error(msg.str());
|
||||
}
|
||||
if (CI.nrows() != n) {
|
||||
msg << "The matrix CI is incompatible (incorrect number of rows "
|
||||
<< CI.nrows() << " , expecting " << n << ")";
|
||||
throw std::logic_error(msg.str());
|
||||
}
|
||||
if (ci0.size() != m) {
|
||||
msg << "The vector ci0 is incompatible (incorrect dimension " << ci0.size()
|
||||
<< ", expecting " << m << ")";
|
||||
throw std::logic_error(msg.str());
|
||||
}
|
||||
x.resize(n);
|
||||
unsigned int i, j, k, l; /* indices */
|
||||
int ip; // this is the index of the constraint to be added to the active set
|
||||
Matrix<double> R(n, n), J(n, n);
|
||||
Vector<double> s(m + p), z(n), r(m + p), d(n), np(n), u(m + p), x_old(n),
|
||||
u_old(m + p);
|
||||
double f_value, psi, c1, c2, sum, ss, R_norm;
|
||||
double inf;
|
||||
if (std::numeric_limits<double>::has_infinity)
|
||||
inf = std::numeric_limits<double>::infinity();
|
||||
else
|
||||
inf = 1.0E300;
|
||||
double t, t1, t2; /* t is the step lenght, which is the minimum of the partial
|
||||
* step length t1 and the full step length t2 */
|
||||
Vector<int> A(m + p), A_old(m + p), iai(m + p);
|
||||
unsigned int iq, iter = 0;
|
||||
Vector<bool> iaexcl(m + p);
|
||||
|
||||
/* p is the number of equality constraints */
|
||||
/* m is the number of inequality constraints */
|
||||
#ifdef TRACE_SOLVER
|
||||
std::cout << std::endl << "Starting solve_quadprog" << std::endl;
|
||||
print_matrix("G", G);
|
||||
print_vector("g0", g0);
|
||||
print_matrix("CE", CE);
|
||||
print_vector("ce0", ce0);
|
||||
print_matrix("CI", CI);
|
||||
print_vector("ci0", ci0);
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Preprocessing phase
|
||||
*/
|
||||
|
||||
/* compute the trace of the original matrix G */
|
||||
c1 = 0.0;
|
||||
for (i = 0; i < n; i++) {
|
||||
c1 += G[i][i];
|
||||
}
|
||||
/* decompose the matrix G in the form L^T L */
|
||||
cholesky_decomposition(G);
|
||||
#ifdef TRACE_SOLVER
|
||||
print_matrix("G", G);
|
||||
#endif
|
||||
/* initialize the matrix R */
|
||||
for (i = 0; i < n; i++) {
|
||||
d[i] = 0.0;
|
||||
for (j = 0; j < n; j++) R[i][j] = 0.0;
|
||||
}
|
||||
R_norm = 1.0; /* this variable will hold the norm of the matrix R */
|
||||
|
||||
/* compute the inverse of the factorized matrix G^-1, this is the initial
|
||||
* value for H */
|
||||
c2 = 0.0;
|
||||
for (i = 0; i < n; i++) {
|
||||
d[i] = 1.0;
|
||||
forward_elimination(G, z, d);
|
||||
for (j = 0; j < n; j++) J[i][j] = z[j];
|
||||
c2 += z[i];
|
||||
d[i] = 0.0;
|
||||
}
|
||||
#ifdef TRACE_SOLVER
|
||||
print_matrix("J", J);
|
||||
#endif
|
||||
|
||||
/* c1 * c2 is an estimate for cond(G) */
|
||||
|
||||
/*
|
||||
* Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
|
||||
* this is a feasible point in the dual space
|
||||
* x = G^-1 * g0
|
||||
*/
|
||||
cholesky_solve(G, x, g0);
|
||||
for (i = 0; i < n; i++) x[i] = -x[i];
|
||||
/* and compute the current solution value */
|
||||
f_value = 0.5 * scalar_product(g0, x);
|
||||
#ifdef TRACE_SOLVER
|
||||
std::cout << "Unconstrained solution: " << f_value << std::endl;
|
||||
print_vector("x", x);
|
||||
#endif
|
||||
|
||||
/* Add equality constraints to the working set A */
|
||||
iq = 0;
|
||||
for (i = 0; i < p; i++) {
|
||||
for (j = 0; j < n; j++) np[j] = CE[j][i];
|
||||
compute_d(d, J, np);
|
||||
update_z(z, J, d, iq);
|
||||
update_r(R, r, d, iq);
|
||||
#ifdef TRACE_SOLVER
|
||||
print_matrix("R", R, n, iq);
|
||||
print_vector("z", z);
|
||||
print_vector("r", r, iq);
|
||||
print_vector("d", d);
|
||||
#endif
|
||||
|
||||
/* compute full step length t2: i.e., the minimum step in primal space s.t.
|
||||
the contraint becomes feasible */
|
||||
t2 = 0.0;
|
||||
if (fabs(scalar_product(z, z)) >
|
||||
std::numeric_limits<double>::epsilon()) // i.e. z != 0
|
||||
t2 = (-scalar_product(np, x) - ce0[i]) / scalar_product(z, np);
|
||||
|
||||
/* set x = x + t2 * z */
|
||||
for (k = 0; k < n; k++) x[k] += t2 * z[k];
|
||||
|
||||
/* set u = u+ */
|
||||
u[iq] = t2;
|
||||
for (k = 0; k < iq; k++) u[k] -= t2 * r[k];
|
||||
|
||||
/* compute the new solution value */
|
||||
f_value += 0.5 * (t2 * t2) * scalar_product(z, np);
|
||||
A[i] = -i - 1;
|
||||
|
||||
if (!add_constraint(R, J, d, iq, R_norm)) {
|
||||
// Equality constraints are linearly dependent
|
||||
throw std::runtime_error("Constraints are linearly dependent");
|
||||
return f_value;
|
||||
}
|
||||
}
|
||||
|
||||
/* set iai = K \ A */
|
||||
for (i = 0; i < m; i++) iai[i] = i;
|
||||
|
||||
l1:
|
||||
iter++;
|
||||
#ifdef TRACE_SOLVER
|
||||
print_vector("x", x);
|
||||
#endif
|
||||
/* step 1: choose a violated constraint */
|
||||
for (i = p; i < iq; i++) {
|
||||
ip = A[i];
|
||||
iai[ip] = -1;
|
||||
}
|
||||
|
||||
/* compute s[x] = ci^T * x + ci0 for all elements of K \ A */
|
||||
ss = 0.0;
|
||||
psi = 0.0; /* this value will contain the sum of all infeasibilities */
|
||||
ip = 0; /* ip will be the index of the chosen violated constraint */
|
||||
for (i = 0; i < m; i++) {
|
||||
iaexcl[i] = true;
|
||||
sum = 0.0;
|
||||
for (j = 0; j < n; j++) sum += CI[j][i] * x[j];
|
||||
sum += ci0[i];
|
||||
s[i] = sum;
|
||||
psi += std::min(0.0, sum);
|
||||
}
|
||||
#ifdef TRACE_SOLVER
|
||||
print_vector("s", s, m);
|
||||
#endif
|
||||
|
||||
if (fabs(psi) <=
|
||||
m * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
|
||||
/* numerically there are not infeasibilities anymore */
|
||||
return f_value;
|
||||
}
|
||||
|
||||
/* save old values for u and A */
|
||||
for (i = 0; i < iq; i++) {
|
||||
u_old[i] = u[i];
|
||||
A_old[i] = A[i];
|
||||
}
|
||||
/* and for x */
|
||||
for (i = 0; i < n; i++) x_old[i] = x[i];
|
||||
|
||||
l2: /* Step 2: check for feasibility and determine a new S-pair */
|
||||
for (i = 0; i < m; i++) {
|
||||
if (s[i] < ss && iai[i] != -1 && iaexcl[i]) {
|
||||
ss = s[i];
|
||||
ip = i;
|
||||
}
|
||||
}
|
||||
if (ss >= 0.0) {
|
||||
return f_value;
|
||||
}
|
||||
|
||||
/* set np = n[ip] */
|
||||
for (i = 0; i < n; i++) np[i] = CI[i][ip];
|
||||
/* set u = [u 0]^T */
|
||||
u[iq] = 0.0;
|
||||
/* add ip to the active set A */
|
||||
A[iq] = ip;
|
||||
|
||||
#ifdef TRACE_SOLVER
|
||||
std::cout << "Trying with constraint " << ip << std::endl;
|
||||
print_vector("np", np);
|
||||
#endif
|
||||
|
||||
l2a: /* Step 2a: determine step direction */
|
||||
/* compute z = H np: the step direction in the primal space (through J, see
|
||||
* the paper) */
|
||||
compute_d(d, J, np);
|
||||
update_z(z, J, d, iq);
|
||||
/* compute N* np (if q > 0): the negative of the step direction in the dual
|
||||
* space */
|
||||
update_r(R, r, d, iq);
|
||||
#ifdef TRACE_SOLVER
|
||||
std::cout << "Step direction z" << std::endl;
|
||||
print_vector("z", z);
|
||||
print_vector("r", r, iq + 1);
|
||||
print_vector("u", u, iq + 1);
|
||||
print_vector("d", d);
|
||||
print_vector("A", A, iq + 1);
|
||||
#endif
|
||||
|
||||
/* Step 2b: compute step length */
|
||||
l = 0;
|
||||
/* Compute t1: partial step length (maximum step in dual space without
|
||||
* violating dual feasibility */
|
||||
t1 = inf; /* +inf */
|
||||
/* find the index l s.t. it reaches the minimum of u+[x] / r */
|
||||
for (k = p; k < iq; k++) {
|
||||
if (r[k] > 0.0) {
|
||||
if (u[k] / r[k] < t1) {
|
||||
t1 = u[k] / r[k];
|
||||
l = A[k];
|
||||
}
|
||||
}
|
||||
}
|
||||
/* Compute t2: full step length (minimum step in primal space such that the
|
||||
* constraint ip becomes feasible */
|
||||
if (fabs(scalar_product(z, z)) >
|
||||
std::numeric_limits<double>::epsilon()) // i.e. z != 0
|
||||
{
|
||||
t2 = -s[ip] / scalar_product(z, np);
|
||||
if (t2 < 0) // patch suggested by Takano Akio for handling numerical
|
||||
// inconsistencies
|
||||
t2 = inf;
|
||||
} else
|
||||
t2 = inf; /* +inf */
|
||||
|
||||
/* the step is chosen as the minimum of t1 and t2 */
|
||||
t = std::min(t1, t2);
|
||||
#ifdef TRACE_SOLVER
|
||||
std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
|
||||
<< ") ";
|
||||
#endif
|
||||
|
||||
/* Step 2c: determine new S-pair and take step: */
|
||||
|
||||
/* case (i): no step in primal or dual space */
|
||||
if (t >= inf) {
|
||||
/* QPP is infeasible */
|
||||
// FIXME: unbounded to raise
|
||||
return inf;
|
||||
}
|
||||
/* case (ii): step in dual space */
|
||||
if (t2 >= inf) {
|
||||
/* set u = u + t * [-r 1] and drop constraint l from the active set A */
|
||||
for (k = 0; k < iq; k++) u[k] -= t * r[k];
|
||||
u[iq] += t;
|
||||
iai[l] = l;
|
||||
delete_constraint(R, J, A, u, n, p, iq, l);
|
||||
#ifdef TRACE_SOLVER
|
||||
std::cout << " in dual space: " << f_value << std::endl;
|
||||
print_vector("x", x);
|
||||
print_vector("z", z);
|
||||
print_vector("A", A, iq + 1);
|
||||
#endif
|
||||
goto l2a;
|
||||
}
|
||||
|
||||
/* case (iii): step in primal and dual space */
|
||||
|
||||
/* set x = x + t * z */
|
||||
for (k = 0; k < n; k++) x[k] += t * z[k];
|
||||
/* update the solution value */
|
||||
f_value += t * scalar_product(z, np) * (0.5 * t + u[iq]);
|
||||
/* u = u + t * [-r 1] */
|
||||
for (k = 0; k < iq; k++) u[k] -= t * r[k];
|
||||
u[iq] += t;
|
||||
#ifdef TRACE_SOLVER
|
||||
std::cout << " in both spaces: " << f_value << std::endl;
|
||||
print_vector("x", x);
|
||||
print_vector("u", u, iq + 1);
|
||||
print_vector("r", r, iq + 1);
|
||||
print_vector("A", A, iq + 1);
|
||||
#endif
|
||||
|
||||
if (fabs(t - t2) < std::numeric_limits<double>::epsilon()) {
|
||||
#ifdef TRACE_SOLVER
|
||||
std::cout << "Full step has taken " << t << std::endl;
|
||||
print_vector("x", x);
|
||||
#endif
|
||||
/* full step has taken */
|
||||
/* add constraint ip to the active set*/
|
||||
if (!add_constraint(R, J, d, iq, R_norm)) {
|
||||
iaexcl[ip] = false;
|
||||
delete_constraint(R, J, A, u, n, p, iq, ip);
|
||||
#ifdef TRACE_SOLVER
|
||||
print_matrix("R", R);
|
||||
print_vector("A", A, iq);
|
||||
print_vector("iai", iai);
|
||||
#endif
|
||||
for (i = 0; i < m; i++) iai[i] = i;
|
||||
for (i = p; i < iq; i++) {
|
||||
A[i] = A_old[i];
|
||||
u[i] = u_old[i];
|
||||
iai[A[i]] = -1;
|
||||
}
|
||||
for (i = 0; i < n; i++) x[i] = x_old[i];
|
||||
goto l2; /* go to step 2 */
|
||||
} else
|
||||
iai[ip] = -1;
|
||||
#ifdef TRACE_SOLVER
|
||||
print_matrix("R", R);
|
||||
print_vector("A", A, iq);
|
||||
print_vector("iai", iai);
|
||||
#endif
|
||||
goto l1;
|
||||
}
|
||||
|
||||
/* a patial step has taken */
|
||||
#ifdef TRACE_SOLVER
|
||||
std::cout << "Partial step has taken " << t << std::endl;
|
||||
print_vector("x", x);
|
||||
#endif
|
||||
/* drop constraint l */
|
||||
iai[l] = l;
|
||||
delete_constraint(R, J, A, u, n, p, iq, l);
|
||||
#ifdef TRACE_SOLVER
|
||||
print_matrix("R", R);
|
||||
print_vector("A", A, iq);
|
||||
#endif
|
||||
|
||||
/* update s[ip] = CI * x + ci0 */
|
||||
sum = 0.0;
|
||||
for (k = 0; k < n; k++) sum += CI[k][ip] * x[k];
|
||||
s[ip] = sum + ci0[ip];
|
||||
|
||||
#ifdef TRACE_SOLVER
|
||||
print_vector("s", s, m);
|
||||
#endif
|
||||
goto l2a;
|
||||
}
|
||||
|
||||
inline void compute_d(Vector<double> &d, const Matrix<double> &J,
|
||||
const Vector<double> &np) {
|
||||
int i, j, n = d.size();
|
||||
double sum;
|
||||
|
||||
/* compute d = H^T * np */
|
||||
for (i = 0; i < n; i++) {
|
||||
sum = 0.0;
|
||||
for (j = 0; j < n; j++) sum += J[j][i] * np[j];
|
||||
d[i] = sum;
|
||||
}
|
||||
}
|
||||
|
||||
inline void update_z(Vector<double> &z, const Matrix<double> &J,
|
||||
const Vector<double> &d, int iq) {
|
||||
int i, j, n = z.size();
|
||||
|
||||
/* setting of z = H * d */
|
||||
for (i = 0; i < n; i++) {
|
||||
z[i] = 0.0;
|
||||
for (j = iq; j < n; j++) z[i] += J[i][j] * d[j];
|
||||
}
|
||||
}
|
||||
|
||||
inline void update_r(const Matrix<double> &R, Vector<double> &r,
|
||||
const Vector<double> &d, int iq) {
|
||||
int i, j;
|
||||
double sum;
|
||||
|
||||
/* setting of r = R^-1 d */
|
||||
for (i = iq - 1; i >= 0; i--) {
|
||||
sum = 0.0;
|
||||
for (j = i + 1; j < iq; j++) sum += R[i][j] * r[j];
|
||||
r[i] = (d[i] - sum) / R[i][i];
|
||||
}
|
||||
}
|
||||
|
||||
bool add_constraint(Matrix<double> &R, Matrix<double> &J, Vector<double> &d,
|
||||
unsigned int &iq, double &R_norm) {
|
||||
unsigned int n = d.size();
|
||||
#ifdef TRACE_SOLVER
|
||||
std::cout << "Add constraint " << iq << '/';
|
||||
#endif
|
||||
unsigned int i, j, k;
|
||||
double cc, ss, h, t1, t2, xny;
|
||||
|
||||
/* we have to find the Givens rotation which will reduce the element
|
||||
d[j] to zero.
|
||||
if it is already zero we don't have to do anything, except of
|
||||
decreasing j */
|
||||
for (j = n - 1; j >= iq + 1; j--) {
|
||||
/* The Givens rotation is done with the matrix (cc cs, cs -cc).
|
||||
If cc is one, then element (j) of d is zero compared with element
|
||||
(j - 1). Hence we don't have to do anything.
|
||||
If cc is zero, then we just have to switch column (j) and column (j - 1)
|
||||
of J. Since we only switch columns in J, we have to be careful how we
|
||||
update d depending on the sign of gs.
|
||||
Otherwise we have to apply the Givens rotation to these columns.
|
||||
The i - 1 element of d has to be updated to h. */
|
||||
cc = d[j - 1];
|
||||
ss = d[j];
|
||||
h = distance(cc, ss);
|
||||
if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0
|
||||
continue;
|
||||
d[j] = 0.0;
|
||||
ss = ss / h;
|
||||
cc = cc / h;
|
||||
if (cc < 0.0) {
|
||||
cc = -cc;
|
||||
ss = -ss;
|
||||
d[j - 1] = -h;
|
||||
} else
|
||||
d[j - 1] = h;
|
||||
xny = ss / (1.0 + cc);
|
||||
for (k = 0; k < n; k++) {
|
||||
t1 = J[k][j - 1];
|
||||
t2 = J[k][j];
|
||||
J[k][j - 1] = t1 * cc + t2 * ss;
|
||||
J[k][j] = xny * (t1 + J[k][j - 1]) - t2;
|
||||
}
|
||||
}
|
||||
/* update the number of constraints added*/
|
||||
iq++;
|
||||
/* To update R we have to put the iq components of the d vector
|
||||
into column iq - 1 of R
|
||||
*/
|
||||
for (i = 0; i < iq; i++) R[i][iq - 1] = d[i];
|
||||
#ifdef TRACE_SOLVER
|
||||
std::cout << iq << std::endl;
|
||||
print_matrix("R", R, iq, iq);
|
||||
print_matrix("J", J);
|
||||
print_vector("d", d, iq);
|
||||
#endif
|
||||
|
||||
if (fabs(d[iq - 1]) <= std::numeric_limits<double>::epsilon() * R_norm) {
|
||||
// problem degenerate
|
||||
return false;
|
||||
}
|
||||
R_norm = std::max<double>(R_norm, fabs(d[iq - 1]));
|
||||
return true;
|
||||
}
|
||||
|
||||
void delete_constraint(Matrix<double> &R, Matrix<double> &J, Vector<int> &A,
|
||||
Vector<double> &u, unsigned int n, int p,
|
||||
unsigned int &iq, int l) {
|
||||
#ifdef TRACE_SOLVER
|
||||
std::cout << "Delete constraint " << l << ' ' << iq;
|
||||
#endif
|
||||
unsigned int i, j, k,
|
||||
qq = 0; // just to prevent warnings from smart compilers
|
||||
double cc, ss, h, xny, t1, t2;
|
||||
|
||||
bool found = false;
|
||||
/* Find the index qq for active constraint l to be removed */
|
||||
for (i = p; i < iq; i++)
|
||||
if (A[i] == l) {
|
||||
qq = i;
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
|
||||
if (!found) {
|
||||
std::ostringstream os;
|
||||
os << "Attempt to delete non existing constraint, constraint: " << l;
|
||||
throw std::invalid_argument(os.str());
|
||||
}
|
||||
/* remove the constraint from the active set and the duals */
|
||||
for (i = qq; i < iq - 1; i++) {
|
||||
A[i] = A[i + 1];
|
||||
u[i] = u[i + 1];
|
||||
for (j = 0; j < n; j++) R[j][i] = R[j][i + 1];
|
||||
}
|
||||
|
||||
A[iq - 1] = A[iq];
|
||||
u[iq - 1] = u[iq];
|
||||
A[iq] = 0;
|
||||
u[iq] = 0.0;
|
||||
for (j = 0; j < iq; j++) R[j][iq - 1] = 0.0;
|
||||
/* constraint has been fully removed */
|
||||
iq--;
|
||||
#ifdef TRACE_SOLVER
|
||||
std::cout << '/' << iq << std::endl;
|
||||
#endif
|
||||
|
||||
if (iq == 0) return;
|
||||
|
||||
for (j = qq; j < iq; j++) {
|
||||
cc = R[j][j];
|
||||
ss = R[j + 1][j];
|
||||
h = distance(cc, ss);
|
||||
if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0
|
||||
continue;
|
||||
cc = cc / h;
|
||||
ss = ss / h;
|
||||
R[j + 1][j] = 0.0;
|
||||
if (cc < 0.0) {
|
||||
R[j][j] = -h;
|
||||
cc = -cc;
|
||||
ss = -ss;
|
||||
} else
|
||||
R[j][j] = h;
|
||||
|
||||
xny = ss / (1.0 + cc);
|
||||
for (k = j + 1; k < iq; k++) {
|
||||
t1 = R[j][k];
|
||||
t2 = R[j + 1][k];
|
||||
R[j][k] = t1 * cc + t2 * ss;
|
||||
R[j + 1][k] = xny * (t1 + R[j][k]) - t2;
|
||||
}
|
||||
for (k = 0; k < n; k++) {
|
||||
t1 = J[k][j];
|
||||
t2 = J[k][j + 1];
|
||||
J[k][j] = t1 * cc + t2 * ss;
|
||||
J[k][j + 1] = xny * (J[k][j] + t1) - t2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline double distance(double a, double b) {
|
||||
double a1, b1, t;
|
||||
a1 = fabs(a);
|
||||
b1 = fabs(b);
|
||||
if (a1 > b1) {
|
||||
t = (b1 / a1);
|
||||
return a1 * sqrt(1.0 + t * t);
|
||||
} else if (b1 > a1) {
|
||||
t = (a1 / b1);
|
||||
return b1 * sqrt(1.0 + t * t);
|
||||
}
|
||||
return a1 * sqrt(2.0);
|
||||
}
|
||||
|
||||
inline double scalar_product(const Vector<double> &x, const Vector<double> &y) {
|
||||
int i, n = x.size();
|
||||
double sum;
|
||||
|
||||
sum = 0.0;
|
||||
for (i = 0; i < n; i++) sum += x[i] * y[i];
|
||||
return sum;
|
||||
}
|
||||
|
||||
void cholesky_decomposition(Matrix<double> &A) {
|
||||
int i, j, k, n = A.nrows();
|
||||
double sum;
|
||||
|
||||
for (i = 0; i < n; i++) {
|
||||
for (j = i; j < n; j++) {
|
||||
sum = A[i][j];
|
||||
for (k = i - 1; k >= 0; k--) sum -= A[i][k] * A[j][k];
|
||||
if (i == j) {
|
||||
if (sum <= 0.0) {
|
||||
std::ostringstream os;
|
||||
// raise error
|
||||
print_matrix("A", A);
|
||||
os << "Error in cholesky decomposition, sum: " << sum;
|
||||
throw std::logic_error(os.str());
|
||||
exit(-1);
|
||||
}
|
||||
A[i][i] = sqrt(sum);
|
||||
} else
|
||||
A[j][i] = sum / A[i][i];
|
||||
}
|
||||
for (k = i + 1; k < n; k++) A[i][k] = A[k][i];
|
||||
}
|
||||
}
|
||||
|
||||
void cholesky_solve(const Matrix<double> &L, Vector<double> &x,
|
||||
const Vector<double> &b) {
|
||||
int n = L.nrows();
|
||||
Vector<double> y(n);
|
||||
|
||||
/* Solve L * y = b */
|
||||
forward_elimination(L, y, b);
|
||||
/* Solve L^T * x = y */
|
||||
backward_elimination(L, x, y);
|
||||
}
|
||||
|
||||
inline void forward_elimination(const Matrix<double> &L, Vector<double> &y,
|
||||
const Vector<double> &b) {
|
||||
int i, j, n = L.nrows();
|
||||
|
||||
y[0] = b[0] / L[0][0];
|
||||
for (i = 1; i < n; i++) {
|
||||
y[i] = b[i];
|
||||
for (j = 0; j < i; j++) y[i] -= L[i][j] * y[j];
|
||||
y[i] = y[i] / L[i][i];
|
||||
}
|
||||
}
|
||||
|
||||
inline void backward_elimination(const Matrix<double> &U, Vector<double> &x,
|
||||
const Vector<double> &y) {
|
||||
int i, j, n = U.nrows();
|
||||
|
||||
x[n - 1] = y[n - 1] / U[n - 1][n - 1];
|
||||
for (i = n - 2; i >= 0; i--) {
|
||||
x[i] = y[i];
|
||||
for (j = i + 1; j < n; j++) x[i] -= U[i][j] * x[j];
|
||||
x[i] = x[i] / U[i][i];
|
||||
}
|
||||
}
|
||||
|
||||
void print_matrix(const char *name, const Matrix<double> &A, int n, int m) {
|
||||
std::ostringstream s;
|
||||
std::string t;
|
||||
if (n == -1) n = A.nrows();
|
||||
if (m == -1) m = A.ncols();
|
||||
|
||||
s << name << ": " << std::endl;
|
||||
for (int i = 0; i < n; i++) {
|
||||
s << " ";
|
||||
for (int j = 0; j < m; j++) s << A[i][j] << ", ";
|
||||
s << std::endl;
|
||||
}
|
||||
t = s.str();
|
||||
t = t.substr(
|
||||
0, t.size() - 3); // To remove the trailing space, comma and newline
|
||||
|
||||
std::cout << t << std::endl;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void print_vector(const char *name, const Vector<T> &v, int n) {
|
||||
std::ostringstream s;
|
||||
std::string t;
|
||||
if (n == -1) n = v.size();
|
||||
|
||||
s << name << ": " << std::endl << " ";
|
||||
for (int i = 0; i < n; i++) {
|
||||
s << v[i] << ", ";
|
||||
}
|
||||
t = s.str();
|
||||
t = t.substr(0, t.size() - 2); // To remove the trailing space and comma
|
||||
|
||||
std::cout << t << std::endl;
|
||||
}
|
||||
} // namespace quadprogpp
|
||||
|
|
@ -3,8 +3,8 @@
|
|||
//
|
||||
|
||||
#include <iostream>
|
||||
#include <unitree_guide_controller/common/interface.h>
|
||||
#include <unitree_guide_controller/robotics/QuadrupedRobot.h>
|
||||
#include "unitree_guide_controller/control/CtrlComponent.h"
|
||||
#include "unitree_guide_controller/robot/QuadrupedRobot.h"
|
||||
|
||||
void QuadrupedRobot::init(const std::string &robot_description) {
|
||||
KDL::Tree robot_tree;
|
||||
|
|
@ -3,7 +3,7 @@
|
|||
//
|
||||
|
||||
#include <memory>
|
||||
#include <unitree_guide_controller/robotics/RobotLeg.h>
|
||||
#include "unitree_guide_controller/robot/RobotLeg.h"
|
||||
|
||||
RobotLeg::RobotLeg(const KDL::Chain &chain) {
|
||||
chain_ = chain;
|
||||
|
|
@ -36,6 +36,6 @@ KDL::JntArray RobotLeg::calcTorque(const KDL::JntArray &joint_positions, const K
|
|||
const KDL::Wrenches &force) const {
|
||||
KDL::JntArray torque(chain_.getNrOfJoints());
|
||||
id_solver_->CartToJnt(joint_positions, joint_velocities, KDL::JntArray(chain_.getNrOfJoints()), force,
|
||||
torque);
|
||||
torque);
|
||||
return torque;
|
||||
}
|
||||
|
|
@ -1,8 +1,7 @@
|
|||
# Controller Manager configuration
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 200 # Hz
|
||||
use_sim_time: true # If running in simulation
|
||||
update_rate: 500 # Hz
|
||||
|
||||
# Define the available controllers
|
||||
joint_state_broadcaster:
|
||||
|
|
@ -11,7 +10,38 @@ controller_manager:
|
|||
imu_sensor_broadcaster:
|
||||
type: imu_sensor_broadcaster/IMUSensorBroadcaster
|
||||
|
||||
unitree_guide_controller:
|
||||
type: unitree_guide_controller/UnitreeGuideController
|
||||
|
||||
imu_sensor_broadcaster:
|
||||
ros__parameters:
|
||||
sensor_name: "imu_sensor"
|
||||
frame_id: "imu_link"
|
||||
|
||||
unitree_guide_controller:
|
||||
ros__parameters:
|
||||
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
|
||||
|
|
@ -3,8 +3,8 @@ 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
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
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
|
||||
|
|
@ -29,40 +29,66 @@ def launch_setup(context, *args, **kwargs):
|
|||
"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],
|
||||
remappings=[
|
||||
("~/robot_description", "/robot_description"),
|
||||
],
|
||||
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=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
|
||||
)
|
||||
|
||||
return [
|
||||
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
|
||||
}
|
||||
],
|
||||
robot_state_publisher,
|
||||
controller_manager,
|
||||
joint_state_publisher,
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_publisher,
|
||||
on_exit=[imu_sensor_broadcaster],
|
||||
)
|
||||
),
|
||||
Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
parameters=[robot_controllers],
|
||||
remappings=[
|
||||
("~/robot_description", "/robot_description"),
|
||||
],
|
||||
output="both",
|
||||
),
|
||||
Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["joint_state_broadcaster",
|
||||
"--controller-manager", "/controller_manager"],
|
||||
),
|
||||
Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["imu_sensor_broadcaster",
|
||||
"--controller-manager", "/controller_manager"],
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=imu_sensor_broadcaster,
|
||||
on_exit=[unitree_guide_controller],
|
||||
)
|
||||
),
|
||||
]
|
||||
|
||||
|
|
|
|||
|
|
@ -15,7 +15,6 @@
|
|||
<exec_depend>joint_state_publisher</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>imu_sensor_broadcaster</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
|
|
|
|||
Loading…
Reference in New Issue