External torque estimation, URDF refining #41
There are no files selected for viewing
| 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 |
| 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 |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,119 @@ | ||
| #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> | ||
|
|
||
| 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++) { | ||
| (*joint_position_)(i) = joint_handles_[i].getPosition(); | ||
| (*joint_velocity_)(i) = joint_handles_[i].getVelocity(); | ||
| (*joint_acceleration_)(i) = 0; | ||
| } | ||
|
|
||
| // Compute Dynamics | ||
| int ret = id_solver_->CartToJnt(*joint_position_, | ||
| *joint_velocity_, | ||
| *joint_acceleration_, | ||
| *joint_wrenches_, | ||
|
There was a problem hiding this comment. 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. 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. 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. 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. 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: There was a problem hiding this comment. 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. 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. |
||
| *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) | ||
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.
The velocity is obtained by derivation of the position, and then smooth filtering, why don't you do the same for the acceleration?
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.
Because I'm lazy 😇
I added it. Does it look correct?
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.
You mean like the whole approach?
Yes, sure, I don't exactly how the KDL solver works, it looks like it gives you all wrenches at all links directly, wonderful.