From 4802885efbe082bdbe8ef7556d8fd198c7a44bfb Mon Sep 17 00:00:00 2001 From: Timon Engelke Date: Fri, 1 Nov 2019 20:14:03 +0100 Subject: [PATCH 1/2] Walking: Effort based gravity compensation --- .../include/bitbots_quintic_walk/walk_ik.h | 7 +- bitbots_quintic_walk/launch/viz.launch | 2 +- bitbots_quintic_walk/src/walk_ik.cpp | 35 +++- bitbots_quintic_walk/src/walk_node.cpp | 4 +- bitbots_splines/CMakeLists.txt | 1 + .../bitbots_splines/dynamic_balancing_goal.h | 2 +- .../bitbots_splines/gravity_compensator.h | 159 ++++++++++++++++++ 7 files changed, 200 insertions(+), 10 deletions(-) create mode 100644 bitbots_splines/include/bitbots_splines/gravity_compensator.h diff --git a/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.h b/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.h index 1b772874..a0af0e46 100644 --- a/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.h +++ b/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.h @@ -1,7 +1,9 @@ #ifndef BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_IK_H_ #define BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_IK_H_ -#include "bitbots_splines/abstract_ik.h" +#include +#include + namespace bitbots_quintic_walk { class WalkIK : public bitbots_splines::AbstractIK { @@ -12,8 +14,9 @@ class WalkIK : public bitbots_splines::AbstractIK { void init(moveit::core::RobotModelPtr kinematic_model) override; void reset(); void setBioIKTimeout(double timeout); - + bool left_support, right_support; private: + moveit::core::RobotModelPtr kinematic_model_; robot_state::RobotStatePtr goal_state_; const moveit::core::JointModelGroup *legs_joints_group_; diff --git a/bitbots_quintic_walk/launch/viz.launch b/bitbots_quintic_walk/launch/viz.launch index a6054eda..3209e152 100644 --- a/bitbots_quintic_walk/launch/viz.launch +++ b/bitbots_quintic_walk/launch/viz.launch @@ -3,7 +3,7 @@ - + diff --git a/bitbots_quintic_walk/src/walk_ik.cpp b/bitbots_quintic_walk/src/walk_ik.cpp index fa16a240..4356e536 100644 --- a/bitbots_quintic_walk/src/walk_ik.cpp +++ b/bitbots_quintic_walk/src/walk_ik.cpp @@ -5,6 +5,7 @@ namespace bitbots_quintic_walk { WalkIK::WalkIK() : bio_ik_timeout_(0.01) {} void WalkIK::init(moveit::core::RobotModelPtr kinematic_model) { + kinematic_model_ = kinematic_model; legs_joints_group_ = kinematic_model->getJointModelGroup("Legs"); goal_state_.reset(new robot_state::RobotState(kinematic_model)); goal_state_->setToDefaultValues(); @@ -19,20 +20,44 @@ bitbots_splines::JointGoals WalkIK::calculate(const std::unique_ptr joint_names = legs_joints_group_->getActiveJointModelNames(); std::vector joint_goals; - goal_state_->copyJointGroupPositions(legs_joints_group_, joint_goals); + goal_state_-> + copyJointGroupPositions(legs_joints_group_, joint_goals + ); /* construct result object */ bitbots_splines::JointGoals result; - result.first = joint_names; - result.second = joint_goals; - return result; + result. + first = joint_names; + result. + second = joint_goals; + return + result; } else { /* maybe do something better here? */ - return bitbots_splines::JointGoals(); + return + bitbots_splines::JointGoals(); } } diff --git a/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_quintic_walk/src/walk_node.cpp index ec61677a..44eb87df 100644 --- a/bitbots_quintic_walk/src/walk_node.cpp +++ b/bitbots_quintic_walk/src/walk_node.cpp @@ -107,6 +107,8 @@ void WalkNode::calculateAndPublishJointGoals(const WalkResponse &response) { std::unique_ptr ik_goals = stabilizer_.stabilize(response); // compute motor goals from IK + ik_.left_support = walk_engine_.isLeftSupport() || walk_engine_.isDoubleSupport(); + ik_.right_support = !walk_engine_.isLeftSupport() || walk_engine_.isDoubleSupport(); bitbots_splines::JointGoals motor_goals = ik_.calculate(std::move(ik_goals)); // publish them @@ -393,7 +395,7 @@ void WalkNode::initializeEngine() { } int main(int argc, char **argv) { - ros::init(argc, argv, "quintic_walking"); + ros::init(argc, argv, "walking"); // init node bitbots_quintic_walk::WalkNode node; diff --git a/bitbots_splines/CMakeLists.txt b/bitbots_splines/CMakeLists.txt index 37d205a6..e9a5a46f 100644 --- a/bitbots_splines/CMakeLists.txt +++ b/bitbots_splines/CMakeLists.txt @@ -49,6 +49,7 @@ set(SOURCES include/bitbots_splines/abstract_visualizer.h include/bitbots_splines/reference_goals.h include/bitbots_splines/dynamic_balancing_goal.h + include/bitbots_splines/gravity_compensator.h ) set(CODE_LIBRARIES) diff --git a/bitbots_splines/include/bitbots_splines/dynamic_balancing_goal.h b/bitbots_splines/include/bitbots_splines/dynamic_balancing_goal.h index a2d6339e..79a90310 100644 --- a/bitbots_splines/include/bitbots_splines/dynamic_balancing_goal.h +++ b/bitbots_splines/include/bitbots_splines/dynamic_balancing_goal.h @@ -73,7 +73,7 @@ class DynamicBalancingGoal : public bio_ik::Goal { // Last element (after all of the regular links) is the reference link bio_ik::Frame reference_link = bio_ik::inverse(context.getLinkFrame(balancing_context_->getLinkCount())); - // static torques from gravity + // static torques from gravity_ for (size_t i = 0; i < balancing_context_->getLinkCount(); i++) { tf2::Vector3 center = balancing_context_->getLinkCenter(i); // m double mass = balancing_context_->getLinkMass(i); // kg diff --git a/bitbots_splines/include/bitbots_splines/gravity_compensator.h b/bitbots_splines/include/bitbots_splines/gravity_compensator.h new file mode 100644 index 00000000..283a6691 --- /dev/null +++ b/bitbots_splines/include/bitbots_splines/gravity_compensator.h @@ -0,0 +1,159 @@ +#pragma once + +#include +#include +#include +#include + +/* + * A class for effort based gravity compensation + */ +class GravityCompensator { + public: + GravityCompensator(moveit::core::RobotModelConstPtr &robot_model) { + efforts_.resize(robot_model->getVariableCount()); + // Initialize link masses and centers + for (auto *link : robot_model->getLinkModels()) { + link_masses_[link] = PointMass(); + if (auto urdf_link = robot_model->getURDF()->getLink(link->getName())) { + if (urdf_link->inertial && urdf_link->inertial->mass > 0) { + link_masses_[link].mass = urdf_link->inertial->mass; + link_masses_[link].center.x() = urdf_link->inertial->origin.position.x; + link_masses_[link].center.y() = urdf_link->inertial->origin.position.y; + link_masses_[link].center.z() = urdf_link->inertial->origin.position.z; + } + } + } + } + + /* + * Compensate gravity + * + * @param state the current robot state + * @param contacts A vector of weighted links contacting the ground + */ + void compensateGravity(moveit::core::RobotStatePtr state, + const std::vector> &contacts) { + std::fill(efforts_.begin(), efforts_.end(), 0); + for (auto &contact: contacts) { + std::string link_name = contact.first; + double contact_weight = contact.second; + const moveit::core::LinkModel *contact_link = state->getRobotModel()->getLinkModel(link_name); + addLink(state, contact_link, nullptr, contact_weight); + } + for (size_t i = 0; i < state->getVariableCount(); i++) { + // P controller to change the position + state->setVariablePosition(i, state->getVariablePosition(i) + 0.02 * efforts_[i]); + } + for (auto *joint: state->getRobotModel()->getJointModels()) { + if (joint->getType() == moveit::core::JointModel::REVOLUTE) { + //ROS_WARN_STREAM(joint->getName() << ": " << efforts_[joint->getFirstVariableIndex()]); + } + } + } + + private: + /* A struct defining a center of mass */ + class PointMass { + public: + Eigen::Vector3d center = Eigen::Vector3d::Zero(); + double mass = 0; + PointMass &operator+=(const PointMass &other) { + center = (center * mass + other.center * other.mass) / (mass + other.mass); + mass += other.mass; + return *this; + } + }; + + /* A map of links to their mass centers */ + std::unordered_map link_masses_; + Eigen::Vector3d gravity_ = Eigen::Vector3d(0, 0, -9.81); + std::vector efforts_; + + /* + * Recursively add all joints to a tree starting from the contact point. + * The contact link is the root of the tree, the connected links are the children and so on. + * Thereby, every joint (i.e. every edge of the tree) supports all the links below it in the tree. + * Efforts on a joint are calculated when all child elements have been added. + * @param state the current robot state + * @param link the link whose children we are adding + * @param coming_joint the joint above the current link in the tree, null if link is the contact link + * @param the weight of the effort + * @returns the center of mass of all links below and including the current link + */ + PointMass addLink(const moveit::core::RobotStatePtr state, + const moveit::core::LinkModel *link, + const moveit::core::JointModel *coming_joint, + double weight) { + /* link_list is a list of the links below the current one in the tree */ + PointMass link_list; + + std::vector connected_joints = link->getChildJointModels(); + connected_joints.push_back(link->getParentJointModel()); + // Iterate over all joints of the current link + for (auto *joint : connected_joints) { + // Skip links already in tree + if (joint == coming_joint) continue; + + // List of all the links below current joints, calculated recursively + PointMass links_below_current_joint; + if (link == joint->getChildLinkModel()) { + // Child already in tree, add parent + if (joint->getParentLinkModel()) { + // parent link is null for base joint + links_below_current_joint = addLink(state, joint->getParentLinkModel(), joint, weight); + } + } else { + // Parent already in tree, add child + links_below_current_joint = addLink(state, joint->getChildLinkModel(), joint, weight); + } + // All child links are added, calculate effort on current joint now if their mass is > 0 + if (links_below_current_joint.mass > 0) { + double effort_on_joint = calculateEffortForJoint(state, joint, links_below_current_joint); + efforts_[joint->getFirstVariableIndex()] += effort_on_joint * weight; + // Add child links of current joint to list of links since they are also below the current link + link_list += links_below_current_joint; + } + } + // Finally add the current link to the list if its mass is larger than 0 + if (link_masses_[link].mass > 0) { + link_list += link_masses_[link]; + } + return link_list; + } + + /* + * calculate the effort on a given joint + * @param state the current robot state + * @param joint the joint of that the effort will be calculated + * @param link_below_current_joint the links that should be considered for effort calculation + * @returns the calculated effort + */ + double calculateEffortForJoint(const robot_state::RobotStatePtr state, + const moveit::core::JointModel *joint, + PointMass links_below_current_joint) { + Eigen::Isometry3d joint_transform; + + // Get rotation of joint + joint->computeTransform(state->getVariablePositions() + joint->getFirstVariableIndex(), joint_transform); + + // parent link is null for the root joint which is in the coordinate system center, use Identity Transform then + Eigen::Isometry3d transform_to_parent = + joint->getParentLinkModel() ? state->getGlobalLinkTransform(joint->getParentLinkModel()) + : Eigen::Isometry3d::Identity(); + + // Get global transform to joint + // child->getJointOriginTransform returns the transform from the parent to the joint + joint_transform = transform_to_parent * joint->getChildLinkModel()->getJointOriginTransform() * joint_transform; + + // We compensate only revolute joints + if (auto *j = dynamic_cast(joint)) { + std::string name = j->getName(); + // Get joint axis in global frame + auto joint_axis = joint_transform.rotation() * j->getAxis(); + // calculate effort on joint axis + return joint_axis.cross(gravity_).dot(links_below_current_joint.center - joint_transform.translation()) * links_below_current_joint.mass; + } + return 0; + } +}; From c84debc121818897c2bdba967f489376cea64e56 Mon Sep 17 00:00:00 2001 From: Timon Engelke Date: Wed, 6 Nov 2019 16:25:40 +0100 Subject: [PATCH 2/2] gravity compensation: fix constructor --- bitbots_splines/include/bitbots_splines/gravity_compensator.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_splines/include/bitbots_splines/gravity_compensator.h b/bitbots_splines/include/bitbots_splines/gravity_compensator.h index 283a6691..c1f76fb9 100644 --- a/bitbots_splines/include/bitbots_splines/gravity_compensator.h +++ b/bitbots_splines/include/bitbots_splines/gravity_compensator.h @@ -10,7 +10,7 @@ */ class GravityCompensator { public: - GravityCompensator(moveit::core::RobotModelConstPtr &robot_model) { + GravityCompensator(moveit::core::RobotModelConstPtr robot_model) { efforts_.resize(robot_model->getVariableCount()); // Initialize link masses and centers for (auto *link : robot_model->getLinkModels()) {