Skip to content

Add support for MuJoCo #597

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 18 commits into
base: main
Choose a base branch
from
Draft
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion bitbots_misc/bitbots_utils/bitbots_utils/utils.py
Original file line number Diff line number Diff line change
@@ -158,7 +158,7 @@ def set_parameters_of_other_node(
return [res.success for res in response.results]


def parse_parameter_dict(*, namespace, parameter_dict):
def parse_parameter_dict(*, namespace, parameter_dict) -> list[ParameterMsg]:
parameters = []
for param_name, param_value in parameter_dict.items():
full_param_name = namespace + param_name
Original file line number Diff line number Diff line change
@@ -1,3 +1,5 @@
from typing import Optional

from biped_interfaces.msg import Phase
from bitbots_quintic_walk_py.libpy_quintic_walk import PyWalkWrapper
from bitbots_utils.utils import parse_parameter_dict
@@ -11,17 +13,24 @@


class PyWalk:
def __init__(self, namespace="", parameters: [Parameter] | None = None, set_force_smooth_step_transition=False):
serialized_parameters = []
if parameters is not None:
for parameter in parameters:
serialized_parameters.append(serialize_message(parameter))
if parameter.value.type == 2:
print(
f"Gave parameter {parameter.name} of integer type. If the code crashes it is maybe because this "
f"should be a float. You may need to add an .0 in some yaml file."
)
self.py_walk_wrapper = PyWalkWrapper(namespace, serialized_parameters, set_force_smooth_step_transition)
def __init__(
self,
namespace="",
walk_parameters: Optional[list[Parameter]] = None,
moveit_parameters: Optional[list[Parameter]] = None,
set_force_smooth_step_transition=False,
):
def serialize_parameters(parameters):
if parameters is None:
return []
return list(map(serialize_message, parameters))

self.py_walk_wrapper = PyWalkWrapper(
namespace,
serialize_parameters(walk_parameters),
serialize_parameters(moveit_parameters),
set_force_smooth_step_transition,
)

def spin_ros(self):
self.py_walk_wrapper.spin_some()
@@ -98,24 +107,24 @@ def set_parameters(self, param_dict):
for parameter in parameters:
self.py_walk_wrapper.set_parameter(serialize_message(parameter))

def get_phase(self):
def get_phase(self) -> float:
return self.py_walk_wrapper.get_phase()

def get_freq(self):
def get_freq(self) -> float:
return self.py_walk_wrapper.get_freq()

def get_support_state(self):
def get_support_state(self) -> Phase:
return deserialize_message(self.py_walk_wrapper.get_support_state(), Phase)

def is_left_support(self):
def is_left_support(self) -> bool:
return self.py_walk_wrapper.is_left_support()

def get_odom(self):
def get_odom(self) -> Odometry:
odom = self.py_walk_wrapper.get_odom()
result = deserialize_message(odom, Odometry)
return result

def publish_debug(self):
def publish_debug(self) -> None:
self.py_walk_wrapper.publish_debug()

def reset_and_test_if_speed_possible(self, cmd_vel_msg, threshold=0.001):
Original file line number Diff line number Diff line change
@@ -54,7 +54,7 @@ namespace bitbots_quintic_walk {
class WalkNode {
public:
explicit WalkNode(rclcpp::Node::SharedPtr node, const std::string &ns = "",
std::vector<rclcpp::Parameter> parameters = {});
const std::vector<rclcpp::Parameter> &moveit_parameters = {});
bitbots_msgs::msg::JointCommand step(double dt);
bitbots_msgs::msg::JointCommand step(double dt, geometry_msgs::msg::Twist::SharedPtr cmdvel_msg,
sensor_msgs::msg::Imu::SharedPtr imu_msg,
@@ -112,8 +112,6 @@ class WalkNode {

nav_msgs::msg::Odometry getOdometry();

rcl_interfaces::msg::SetParametersResult onSetParameters(const std::vector<rclcpp::Parameter> &parameters);

void publish_debug();
rclcpp::TimerBase::SharedPtr startTimer();
double getTimerFreq();
Original file line number Diff line number Diff line change
@@ -23,7 +23,8 @@ using namespace ros2_python_extension;

class PyWalkWrapper {
public:
explicit PyWalkWrapper(std::string ns, std::vector<py::bytes> parameter_msgs = {},
explicit PyWalkWrapper(const std::string &ns, const std::vector<py::bytes> &walk_parameter_msgs = {},
const std::vector<py::bytes> &moveit_parameter_msgs = {},
bool force_smooth_step_transition = false);
py::bytes step(double dt, py::bytes &cmdvel_msg, py::bytes &imu_msg, py::bytes &jointstate_msg,
py::bytes &pressure_left, py::bytes &pressure_right);
27 changes: 11 additions & 16 deletions bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp
Original file line number Diff line number Diff line change
@@ -9,29 +9,24 @@ using namespace std::chrono_literals;

namespace bitbots_quintic_walk {

WalkNode::WalkNode(rclcpp::Node::SharedPtr node, const std::string& ns, std::vector<rclcpp::Parameter> parameters)
WalkNode::WalkNode(rclcpp::Node::SharedPtr node, const std::string& ns,
const std::vector<rclcpp::Parameter>& moveit_parameters)
: node_(node),
param_listener_(node_),
config_(param_listener_.get_params()),
walk_engine_(node_, config_.engine),
stabilizer_(node_),
ik_(node_, config_.node.ik),
visualizer_(node_, config_.node.tf) {
// Create dummy node for moveit
auto moveit_node = std::make_shared<rclcpp::Node>(ns + "walking_moveit_node");

// when called from python, parameters are given to the constructor
for (auto parameter : parameters) {
if (node_->has_parameter(parameter.get_name())) {
// this is the case for walk engine params set via python
node_->set_parameter(parameter);
} else {
// parameter is not for the walking, set on moveit node
moveit_node->declare_parameter(parameter.get_name(), parameter.get_type());
moveit_node->set_parameter(parameter);
}
}

// Create dummy node for moveit. This is necessary for dynamic reconfigure to work (moveit does some bullshit with
// parameter declarations, so we need to isolate the walking parameters from the moveit parameters).
// If the walking is instantiated using the python wrapper, moveit parameters are passed because no moveit config
// is loaded in the conventional way. Normally the moveit config is loaded via launch file and the passed vector is
// empty.
auto moveit_node = std::make_shared<rclcpp::Node>(
"walking_moveit_node", ns,
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true).parameter_overrides(
moveit_parameters));
// get all kinematics parameters from the move_group node if they are not set manually via constructor
std::string check_kinematic_parameters;
if (!moveit_node->get_parameter("robot_description_kinematics.LeftLeg.kinematics_solver",
38 changes: 27 additions & 11 deletions bitbots_motion/bitbots_quintic_walk/src/walk_pywrapper.cpp
Original file line number Diff line number Diff line change
@@ -2,21 +2,37 @@

void PyWalkWrapper::spin_some() { rclcpp::spin_some(node_); }

PyWalkWrapper::PyWalkWrapper(std::string ns, std::vector<py::bytes> parameter_msgs, bool force_smooth_step_transition) {
PyWalkWrapper::PyWalkWrapper(const std::string &ns, const std::vector<py::bytes> &walk_parameter_msgs,
const std::vector<py::bytes> &moveit_parameter_msgs, bool force_smooth_step_transition) {
// initialize rclcpp if not already done
if (!rclcpp::contexts::get_global_default_context()->is_valid()) {
rclcpp::init(0, nullptr);
}

// create parameters from serialized messages
std::vector<rclcpp::Parameter> cpp_parameters = {};
for (auto &parameter_msg : parameter_msgs) {
cpp_parameters.push_back(
rclcpp::Parameter::from_parameter_msg(fromPython<rcl_interfaces::msg::Parameter>(parameter_msg)));
}

node_ = rclcpp::Node::make_shared(ns + "walking");
walk_node_ = std::make_shared<bitbots_quintic_walk::WalkNode>(node_, ns, cpp_parameters);
// internal function to deserialize the parameter messages
auto deserialize_parameters = [](std::vector<py::bytes> parameter_msgs) {
std::vector<rclcpp::Parameter> cpp_parameters = {};
for (auto &parameter_msg : parameter_msgs) {
cpp_parameters.push_back(
rclcpp::Parameter::from_parameter_msg(fromPython<rcl_interfaces::msg::Parameter>(parameter_msg)));
}
return cpp_parameters;
};

// Create a node object
// Even tho we use python bindings instead of ros's dds, we still need a node object for logging and parameter
// handling Because the walking is not started using the launch infrastructure and an appropriate parameter file, we
// need to manually set the parameters
node_ = rclcpp::Node::make_shared(
"walking", ns, rclcpp::NodeOptions().parameter_overrides(deserialize_parameters(walk_parameter_msgs)));

// Create the walking object
// We pass it the node we created. But the walking also creates a helper node for moveit (otherwise dynamic
// reconfigure does not work, because moveit does some bullshit with their parameter declarations leading dynamic
// reconfigure not working). This way the walking parameters are isolated from the moveit parameters, allowing dynamic
// reconfigure to work. Therefore we need to pass the moveit parameters to the walking.
walk_node_ =
std::make_shared<bitbots_quintic_walk::WalkNode>(node_, ns, deserialize_parameters(moveit_parameter_msgs));
set_robot_state(0);
walk_node_->initializeEngine();
walk_node_->getEngine()->setForceSmoothStepTransition(force_smooth_step_transition);
@@ -197,7 +213,7 @@ PYBIND11_MODULE(libpy_quintic_walk, m) {
using namespace bitbots_quintic_walk;

py::class_<PyWalkWrapper, std::shared_ptr<PyWalkWrapper>>(m, "PyWalkWrapper")
.def(py::init<std::string, std::vector<py::bytes>, bool>())
.def(py::init<std::string, std::vector<py::bytes>, std::vector<py::bytes>, bool>())
.def("step", &PyWalkWrapper::step)
.def("step_relative", &PyWalkWrapper::step_relative)
.def("step_open_loop", &PyWalkWrapper::step_open_loop)
1 change: 1 addition & 0 deletions bitbots_wolfgang/wolfgang_description/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -59,5 +59,6 @@ ament_export_include_directories(${INCLUDE_DIRS})
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME})
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
install(DIRECTORY scripts DESTINATION share/${PROJECT_NAME})

ament_package()
Loading