-
Notifications
You must be signed in to change notification settings - Fork 80
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
External torque estimation, URDF refining #41
Closed
marcoesposito1988
wants to merge
15
commits into
CentroEPiaggio:master
from
marcoesposito1988:arm_state_controller
Closed
Changes from 9 commits
Commits
Show all changes
15 commits
Select commit
Hold shift + click to select a range
c9b21d4
Started working on ArmStateController
marcoesposito1988 acbd69d
Compiling ArmStateController
marcoesposito1988 654ee12
Not-exploding ArmStateController
marcoesposito1988 c8b1007
Updated URDF link mass
marcoesposito1988 6f31382
WIP: estimation of cartesian wrench on end effector
marcoesposito1988 80f5a42
Transform est_ext_wrench to tool reference frame
marcoesposito1988 73e4211
Using jacobian transpose pseudo-inverse
marcoesposito1988 030653e
Added external torque in base reference frame
marcoesposito1988 233a33e
Derive acceleration through exp smoothing
marcoesposito1988 b962cff
Fix bad copy-paste!
marcoesposito1988 5c824d6
Use damped SVD decomposition for better stability at singularities
marcoesposito1988 6645055
Optional rviz for moveit in single_lwr.launch
marcoesposito1988 d5d8fb8
Fix parenthesis in lwr_description/utils.xacro
marcoesposito1988 15074f4
Fixed damping initial value to avoid vibrations
marcoesposito1988 21fc0b4
Fixed compilation error on 16.04
marcoesposito1988 File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
48 changes: 48 additions & 0 deletions
48
lwr_controllers/include/lwr_controllers/arm_state_controller.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,48 @@ | ||
| #ifndef ARM_STATE_CONTROLLER_H | ||
| #define ARM_STATE_CONTROLLER_H | ||
|
|
||
| #include "KinematicChainControllerBase.h" | ||
|
|
||
| #include <lwr_controllers/ArmState.h> | ||
| #include <std_msgs/Float32.h> | ||
| #include <std_msgs/Bool.h> | ||
|
|
||
| #include <realtime_tools/realtime_publisher.h> | ||
|
|
||
| #include <boost/scoped_ptr.hpp> | ||
|
|
||
| namespace arm_state_controller | ||
| { | ||
| class ArmStateController: public controller_interface::KinematicChainControllerBase<hardware_interface::JointStateInterface> | ||
| { | ||
| public: | ||
|
|
||
| ArmStateController(); | ||
| ~ArmStateController(); | ||
|
|
||
| bool init(hardware_interface::JointStateInterface *robot, ros::NodeHandle &n); | ||
| void starting(const ros::Time& time); | ||
| void update(const ros::Time& time, const ros::Duration& period); | ||
| void stopping(const ros::Time& time); | ||
|
|
||
| private: | ||
|
|
||
| boost::shared_ptr< realtime_tools::RealtimePublisher< lwr_controllers::ArmState > > realtime_pub_; | ||
|
|
||
| boost::scoped_ptr<KDL::ChainIdSolver_RNE> id_solver_; | ||
| boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_; | ||
| boost::scoped_ptr<KDL::ChainFkSolverPos> fk_solver_; | ||
| boost::scoped_ptr<KDL::Jacobian> jacobian_; | ||
| boost::scoped_ptr<KDL::Vector> gravity_; | ||
| boost::scoped_ptr<KDL::JntArray> joint_position_; | ||
| boost::scoped_ptr<KDL::JntArray> joint_velocity_; | ||
| boost::scoped_ptr<KDL::JntArray> joint_acceleration_; | ||
| boost::scoped_ptr<KDL::Wrenches> joint_wrenches_; | ||
| boost::scoped_ptr<KDL::JntArray> joint_effort_est_; | ||
|
|
||
| ros::Time last_publish_time_; | ||
| double publish_rate_; | ||
| }; | ||
| } | ||
|
|
||
| #endif |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,6 @@ | ||
| Header header | ||
|
|
||
| string[] joint_name | ||
| float32[] est_ext_torques | ||
| geometry_msgs/Wrench est_ee_wrench | ||
| geometry_msgs/Wrench est_ee_wrench_base |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,121 @@ | ||
| #include <pluginlib/class_list_macros.h> | ||
| #include <math.h> | ||
|
|
||
| #include <lwr_controllers/arm_state_controller.h> | ||
| #include <kdl_conversions/kdl_msg.h> | ||
| #include <utils/pseudo_inversion.h> | ||
| #include <control_toolbox/filters.h> | ||
|
|
||
| namespace arm_state_controller | ||
| { | ||
| ArmStateController::ArmStateController() {} | ||
| ArmStateController::~ArmStateController() {} | ||
|
|
||
| bool ArmStateController::init(hardware_interface::JointStateInterface *robot, ros::NodeHandle &n) | ||
| { | ||
| KinematicChainControllerBase<hardware_interface::JointStateInterface>::init(robot, n); | ||
|
|
||
| // get publishing period | ||
| if (!nh_.getParam("publish_rate", publish_rate_)) { | ||
| ROS_ERROR("Parameter 'publish_rate' not set"); | ||
| return false; | ||
| } | ||
|
|
||
| realtime_pub_.reset(new realtime_tools::RealtimePublisher<lwr_controllers::ArmState>(nh_,"arm_state",4)); | ||
| realtime_pub_->msg_.est_ext_torques.resize(kdl_chain_.getNrOfJoints()); | ||
|
|
||
| gravity_.reset(new KDL::Vector(0.0, 0.0, -9.81)); // TODO: compute from actual robot position (TF?) | ||
| id_solver_.reset(new KDL::ChainIdSolver_RNE(kdl_chain_, *gravity_)); | ||
| jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_)); | ||
| fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_)); | ||
| jacobian_.reset(new KDL::Jacobian(kdl_chain_.getNrOfJoints())); | ||
| joint_position_.reset(new KDL::JntArray(kdl_chain_.getNrOfJoints())); | ||
| joint_velocity_.reset(new KDL::JntArray(kdl_chain_.getNrOfJoints())); | ||
| joint_acceleration_.reset(new KDL::JntArray(kdl_chain_.getNrOfJoints())); | ||
| joint_wrenches_.reset(new KDL::Wrenches(kdl_chain_.getNrOfJoints())); | ||
| joint_effort_est_.reset(new KDL::JntArray(kdl_chain_.getNrOfJoints())); | ||
|
|
||
| return true; | ||
| } | ||
|
|
||
| void ArmStateController::starting(const ros::Time& time) | ||
| { | ||
| last_publish_time_ = time; | ||
| for (unsigned i = 0; i < joint_handles_.size(); i++){ | ||
| (*joint_position_)(i) = joint_handles_[i].getPosition(); | ||
| (*joint_velocity_)(i) = joint_handles_[i].getVelocity(); | ||
| (*joint_acceleration_)(i) = 0; | ||
| } | ||
| ROS_INFO("started"); | ||
| } | ||
|
|
||
| void ArmStateController::update(const ros::Time& time, const ros::Duration& period) | ||
| { | ||
| // limit rate of publishing | ||
| if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0/publish_rate_) < time){ | ||
|
|
||
| if (realtime_pub_->trylock()) { | ||
| // we're actually publishing, so increment time | ||
| last_publish_time_ = last_publish_time_ + ros::Duration(1.0/publish_rate_); | ||
|
|
||
| for (unsigned i = 0; i < joint_handles_.size(); i++) { | ||
| float acceleration = filters::exponentialSmoothing((joint_handles_[i].getPosition() - (*joint_position_)(i))/period.toSec(), joint_handles_[i].getVelocity(), 0.2); | ||
| (*joint_position_)(i) = joint_handles_[i].getPosition(); | ||
| (*joint_velocity_)(i) = joint_handles_[i].getVelocity(); | ||
| (*joint_acceleration_)(i) = acceleration; | ||
| } | ||
|
|
||
| // Compute Dynamics | ||
| int ret = id_solver_->CartToJnt(*joint_position_, | ||
| *joint_velocity_, | ||
| *joint_acceleration_, | ||
| *joint_wrenches_, | ||
| *joint_effort_est_); | ||
| if (ret < 0) { | ||
| ROS_ERROR("KDL: inverse dynamics ERROR"); | ||
| realtime_pub_->unlock(); | ||
| return; | ||
| } | ||
|
|
||
| realtime_pub_->msg_.header.stamp = time; | ||
| for (unsigned i=0; i<joint_handles_.size(); i++) { | ||
| realtime_pub_->msg_.est_ext_torques[i] = joint_handles_[i].getEffort() - (*joint_effort_est_)(i); | ||
| } | ||
|
|
||
| // Compute cartesian wrench on end effector | ||
| ret = jac_solver_->JntToJac(*joint_position_, *jacobian_); | ||
| if (ret < 0) { | ||
| ROS_ERROR("KDL: jacobian computation ERROR"); | ||
| realtime_pub_->unlock(); | ||
| return; | ||
| } | ||
|
|
||
| Eigen::MatrixXd jinv; | ||
| pseudo_inverse(jacobian_->data.transpose(),jinv,false); | ||
|
|
||
| KDL::Wrench wrench; | ||
|
|
||
| for (unsigned int i = 0; i < 6; i++) | ||
| for (unsigned int j = 0; j < kdl_chain_.getNrOfJoints(); j++) | ||
| wrench[i] += jinv(i,j) * realtime_pub_->msg_.est_ext_torques[j]; | ||
|
|
||
| tf::wrenchKDLToMsg(wrench, realtime_pub_->msg_.est_ee_wrench_base); | ||
|
|
||
| // Transform cartesian wrench into tool reference frame | ||
| KDL::Frame tool_frame; | ||
| fk_solver_->JntToCart(*joint_position_, tool_frame); | ||
| KDL::Wrench tool_wrench = tool_frame * wrench; | ||
|
|
||
| tf::wrenchKDLToMsg(tool_wrench, realtime_pub_->msg_.est_ee_wrench); | ||
|
|
||
| realtime_pub_->unlockAndPublish(); | ||
| } | ||
| } | ||
| } | ||
|
|
||
| void ArmStateController::stopping(const ros::Time& time) | ||
| {} | ||
|
|
||
| } | ||
|
|
||
| PLUGINLIB_EXPORT_CLASS(arm_state_controller::ArmStateController, controller_interface::ControllerBase) | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
But I'm reviewing still...
According to the api, this joint_wrenches_ is an input, right? Where do you update it?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I suppose you are using joint_wrenches_= 0 always, so it is just to estimate the torque at the joints from the model, is that it?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
If so, I think that the class you should use is ChainDynParam, I couldn't find a forward dynamic solver.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Yes, I use the inverse dynamics solver to find the expected joint torques given the current position, velocity and acceleration of the robot. Then I subtract the expected from the measured joint torques to find the estimated external joint torques.
So joint_wrenches is input, and I set it to zero to find the expected joint torques when there is no object applying a force on the robot.
I think that the inverse dynamics solver is a higher-level solver than ChainDynParam, and is closer to what we want. Or do you see any problem with this approach?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
No, it's fine, I was just wondering that if you never consider external forces (joint_wrenhes_ = 0), perhaps there was no need to use the solver, because you are not actually solving neither a FD nor an ID problem, also because the motion is measured not planned, but the numbers are the same as in an ID problem.
So, as you wish, no problem, let me know when to merge.
Though, I'd change the var name in case we ever use it, since it is not joint wrenches as if the joint generated the wrenches, but wrenches at the segments, I believe expressed in the corresponding joint frame from this, it doesn't say that, only:
f_ext The external forces (no gravity) on the segments Output parameters:anyways, it is zero in our case.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ok. I'm still playing with it because today I get very crappy performance out of the controller. I don't know if I changed something wrong; in the meantime I realised that I was computing the acceleration in a very wrong way 😇 so I fixed it
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
And I might have found the reason.. I think that the numerical stability was due to using the damped pseudo-inverse of the Jacobian. Somehow the change did not make it into the git commit.
I would say that we can test it a bit more, and then merge it.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
👍