Skip to content
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

Add admittance controller #370

Merged
merged 128 commits into from
Oct 10, 2022
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
128 commits
Select commit Hold shift + click to select a range
0206ee6
Make forward controller chainable.
destogl Mar 21, 2022
43f39a0
Enabling chanable mode for forward command controllers.
destogl Mar 23, 2022
29c4393
Fix all bugs in chained-controllers mode.
destogl Mar 24, 2022
6f2f956
Remove debug output from forwarding controller.
destogl Mar 29, 2022
704ba5d
Make forward controller chainable.
destogl Mar 21, 2022
6759bb5
Enabling chanable mode for forward command controllers.
destogl Mar 23, 2022
7afdefc
Fix all bugs in chained-controllers mode.
destogl Mar 24, 2022
7c03f79
Remove debug output from forwarding controller.
destogl Mar 29, 2022
f8556ea
Some compile fixes
destogl Apr 26, 2022
d77b4b6
Fix some small issues.
destogl Apr 26, 2022
778ed13
Update to use class.
destogl May 16, 2022
39b640e
added admittance controller
pac48 Jun 19, 2022
be2f308
merge fwd chainable controller
pac48 Jun 20, 2022
4f3d709
merged additional from fwd chainable controllers
pac48 Jun 20, 2022
fcc2be2
fixed interface memory bug
pac48 Jun 20, 2022
0aac28e
changed position/velocity reference fields
pac48 Jun 20, 2022
7033048
added gravity compensation
pac48 Jun 20, 2022
1a266bc
replaced Eigen 3.4 API with Eigen 3.3
pac48 Jun 20, 2022
8dc9b37
clean up and TODOs
pac48 Jun 20, 2022
1563113
removed reshaped operation and fix forward velocity
pac48 Jun 21, 2022
5d59427
code cleanup and refactored gravity compensation
pac48 Jun 22, 2022
18097a2
replaced parameter handler with auto-generated struct
pac48 Jun 25, 2022
dfc27bf
dynamic parameter updating enabled
pac48 Jun 25, 2022
2db281f
enable/disable parameter update during execution + fill state message
pac48 Jun 27, 2022
9990dca
remove unused code
pac48 Jun 28, 2022
690b89b
eliminate ik drift: broken
pac48 Jun 28, 2022
0e6cfb1
correct drift from integrating position
pac48 Jun 28, 2022
79a54dc
working drift correction for position and rotation
pac48 Jun 29, 2022
23ed194
Merge branch 'master' into add-admittance-controller
pac48 Jun 30, 2022
c54d46c
fix pparameter copying behaviour
pac48 Jun 30, 2022
906c602
add parameter field for IK package
pac48 Jul 1, 2022
4a6f779
fix struct formatting
pac48 Jul 1, 2022
84ccdde
enable tests: currently failing
pac48 Jul 4, 2022
976d286
updated auto generated struct with parameter validation
pac48 Jul 5, 2022
965c226
added parameter validation and descriptions
pac48 Jul 6, 2022
a0f4bae
general code clean up
pac48 Jul 6, 2022
27ea60f
parameter handling tests passing
pac48 Jul 7, 2022
737363c
declare parameters in test
pac48 Jul 19, 2022
6936c5d
change parameter struct format
pac48 Jul 27, 2022
382a1b6
passing tests
pac48 Jul 28, 2022
cbd124c
add error checks on all public methods
pac48 Jul 28, 2022
967f64f
code clean up
pac48 Jul 28, 2022
bcac129
added parameters for drift correction
pac48 Jul 31, 2022
7b29386
Update controllers with new get_name hardware interfaces (#369)
Schulze18 Jul 2, 2022
da52b32
integrate motion in joint space to eliminate drift error
pac48 Aug 3, 2022
0951605
remove drift correction parameters
pac48 Aug 3, 2022
f2acf62
Merge branch 'add-admittance-controller' of https://github.com/pac48/…
pac48 Aug 3, 2022
9885432
move interface vector reset to on_activate
pac48 Aug 3, 2022
7d5fcf2
remove interface maps
pac48 Aug 4, 2022
f19b096
Merge branch 'master' of https://github.com/ros-controls/ros2_control…
pac48 Aug 4, 2022
cf7031f
fix JTC with newest ros_control
pac48 Aug 4, 2022
bdb32b4
mend
pac48 Aug 5, 2022
ed37061
revert forward command controller changes
pac48 Aug 5, 2022
c7c7312
update force reading in hardware read function
pac48 Aug 5, 2022
7a3ecf9
Merge branch 'add-admittance-controller' of https://github.com/pac48/…
pac48 Aug 5, 2022
45b8afd
initialize reference vectors properly
pac48 Aug 6, 2022
893cea6
resize states in activate
pac48 Aug 6, 2022
13959e1
Merge branch 'add-admittance-controller' of https://github.com/pac48/…
pac48 Aug 5, 2022
911ecdd
use reference wrapper instead of double pointer
pac48 Aug 8, 2022
d8999a5
remove rotation struct
pac48 Aug 8, 2022
6c4bf66
Merge branch 'add-admittance-controller' of github.com:pac48/ros2_con…
pac48 Aug 8, 2022
4833f4a
update test
pac48 Aug 8, 2022
7f22790
remove generated code
pac48 Aug 8, 2022
451f6d4
use Eigen tranformation types
pac48 Aug 9, 2022
8c1bcf4
update state message
pac48 Aug 9, 2022
bff17df
remove warnings
pac48 Aug 9, 2022
2c1282e
implement state message information and enable selected axes
pac48 Aug 10, 2022
cc90178
add test
pac48 Aug 10, 2022
3086290
Merge branch 'master' of https://github.com/ros-controls/ros2_control…
pac48 Aug 10, 2022
d850b47
update README
pac48 Aug 10, 2022
51e85c6
update variable names
pac48 Aug 10, 2022
1541318
update kinematics interface use
pac48 Aug 11, 2022
932a554
Make tricycle controller the same as master.
destogl Aug 20, 2022
a15728e
Revert "Make tricycle controller the same as master."
destogl Aug 20, 2022
f97f7d9
Merge remote-tracking branch 'upstream/master' into add-admittance-co…
destogl Aug 20, 2022
ccec03d
Correct formatting and linting issues.
destogl Aug 21, 2022
01f5e3e
Merge remote-tracking branch 'upstream/master' into add-admittance-co…
destogl Aug 21, 2022
9760b92
JTC should be the same as on master.
destogl Aug 21, 2022
6ba8bcc
A bit more code cleaning and polishing.
destogl Aug 21, 2022
9a08f77
update control_msgs repo
pac48 Aug 22, 2022
8011760
Merge branch 'add-admittance-controller' of github.com:pac48/ros2_con…
pac48 Aug 22, 2022
4b0b4b2
remove joint limits from package
pac48 Aug 22, 2022
8f85ead
add kinematics_interface to not released .repo files
pac48 Aug 22, 2022
742b252
add tf2_kdl to admittance package.xml
pac48 Aug 22, 2022
9774584
add kinematics_interface to all .repo files
pac48 Aug 22, 2022
a225c17
add generate_parameter_library to .repos
pac48 Aug 22, 2022
dc1641b
fix url
pac48 Aug 22, 2022
84fa4fe
add control_msgs to not-released .repos
pac48 Aug 22, 2022
8ae5090
update kinematics plugin name
pac48 Aug 22, 2022
e323d14
fix parameter update on restart
pac48 Aug 22, 2022
9c148a0
update repos
pac48 Aug 22, 2022
795cdce
Precommit
pac48 Aug 22, 2022
2f609be
update repos
pac48 Aug 22, 2022
7710448
fix test output
pac48 Aug 23, 2022
a5847a2
move parameter initialization to controller class
pac48 Aug 23, 2022
d8674cd
use single admittance transforms struct
pac48 Aug 23, 2022
d1d42df
rename cog_ and ee_weight_ variables
pac48 Aug 23, 2022
d6050dd
Update admittance_controller/include/admittance_controller/admittance…
pac48 Aug 23, 2022
93bd627
reset on values deactivate
pac48 Aug 23, 2022
ac307ce
fix admittance reset
pac48 Aug 23, 2022
ed0b014
add joint damping parameter
pac48 Aug 23, 2022
ba73aee
update documenation in admittance_rule
pac48 Aug 23, 2022
c61217b
Update comments
pac48 Aug 23, 2022
dacd8bb
update descriptions for the admittance parameters
pac48 Aug 23, 2022
ce583b0
refactor indexing to be like JTC
pac48 Aug 23, 2022
d814028
clear joint_command_interface_ and joint_state_interface_ in deactivate
pac48 Aug 24, 2022
c4ae3da
fix include for rolling
pac48 Aug 24, 2022
3dc1816
update README
pac48 Aug 24, 2022
3287740
update comments
pac48 Aug 24, 2022
bde3d39
Update admittance_controller/include/admittance_controller/admittance…
pac48 Aug 24, 2022
f23b6a6
Update admittance_controller/src/admittance_controller.cpp
pac48 Aug 24, 2022
bbdc506
Update admittance_controller/src/admittance_controller.cpp
pac48 Aug 24, 2022
c45b668
move dynamic allocations in state message to reset method
pac48 Aug 24, 2022
a4817c1
Merge branch 'add-admittance-controller' of github.com:pac48/ros2_con…
pac48 Aug 24, 2022
ea59076
Update admittance_controller/src/admittance_controller.cpp
pac48 Aug 24, 2022
f94ca25
Update admittance_controller/include/admittance_controller/admittance…
pac48 Aug 24, 2022
6a0281f
Remove unneeded code and chnage field name
pac48 Aug 24, 2022
4346e3d
update comments/docs
pac48 Aug 24, 2022
82c0018
remove tmp variables
pac48 Aug 24, 2022
2855cc7
Merge branch 'master' into add-admittance-controller
destogl Aug 31, 2022
91af1ac
Merge branch 'master' into add-admittance-controller
Sep 23, 2022
2644a4e
Improve some comments
Sep 23, 2022
ec55e5c
Merge branch 'master' into add-admittance-controller
destogl Oct 6, 2022
08b3909
Merge branch 'master' into add-admittance-controller
destogl Oct 10, 2022
83c6e5d
Updated documentation.
destogl Oct 10, 2022
2e57e4a
Fixing linters.
destogl Oct 10, 2022
28c5665
Small polishing
destogl Oct 10, 2022
e9c7432
Update upstream repo.
destogl Oct 10, 2022
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
Prev Previous commit
Next Next commit
clean up and TODOs
  • Loading branch information
pac48 committed Jun 20, 2022
commit 8dc9b37eb8d2d3ab31edce010907dc708bae5bb9
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,6 @@ namespace admittance_controller
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

struct RTBuffers{
realtime_tools::RealtimeBuffer<std::shared_ptr<geometry_msgs::msg::WrenchStamped>> input_wrench_command_;
realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint>> input_joint_command_;
std::unique_ptr<realtime_tools::RealtimePublisher<ControllerStateMsg>> state_publisher_;
};
Expand Down Expand Up @@ -113,31 +112,28 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
bool on_set_chained_mode(bool chained_mode) override;

int num_joints_{};
destogl marked this conversation as resolved.
Show resolved Hide resolved

std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_position_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_velocity_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_acceleration_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_effort_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> joint_position_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> joint_velocity_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> joint_acceleration_state_interface_;
// std::vector<std::reference_wrapper<hardware_interface::CommandInterface>> joint_position_chainable_interface_;
// std::vector<std::reference_wrapper<hardware_interface::CommandInterface>> joint_velocity_chainable_interface_;

std::vector<double*> position_reference_;
std::vector<double*> velocity_reference_;
std::vector<std::reference_wrapper<hardware_interface::CommandInterface>> joint_acceleration_chainable_interface_;

// Admittance rule and dependent variables;
std::unique_ptr<admittance_controller::AdmittanceRule> admittance_;
// joint limiter TODO
destogl marked this conversation as resolved.
Show resolved Hide resolved
std::unique_ptr<semantic_components::ForceTorqueSensor> force_torque_sensor_;
// controller parameters filled by ROS
ParameterStruct params;
// ROS subscribers
rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr input_wrench_command_subscriber_ = nullptr;
rclcpp::Subscription<trajectory_msgs::msg::JointTrajectoryPoint>::SharedPtr input_joint_command_subscriber_ = nullptr;
rclcpp::Publisher<control_msgs::msg::AdmittanceControllerState>::SharedPtr s_publisher_ = nullptr;
// ROS messages
std::shared_ptr<trajectory_msgs::msg::JointTrajectory> traj_command_msg;
std::shared_ptr<geometry_msgs::msg::WrenchStamped> wrench_msg;
std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint> joint_command_msg;
pac48 marked this conversation as resolved.
Show resolved Hide resolved
// real-time buffer
RTBuffers rtBuffers;
destogl marked this conversation as resolved.
Show resolved Hide resolved
Expand All @@ -159,11 +155,10 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
std::vector<double> open_loop_buffer;

// helper methods
void wrench_stamped_callback(const std::shared_ptr<geometry_msgs::msg::WrenchStamped> msg);
void joint_command_callback(const std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint> msg);
void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint & state);
void read_state_from_command_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state);
void read_state_reference_from_chainable_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state);
void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state);

destogl marked this conversation as resolved.
Show resolved Hide resolved
};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ namespace { // Utility namespace
static constexpr double WRENCH_EPSILON = 1e-10;
static constexpr double POSE_ERROR_EPSILON = 1e-12;
static constexpr double POSE_EPSILON = 1e-15;
const double ROT_AXIS_EPSILON = 0.001;


} // utility namespace
Expand All @@ -46,9 +47,9 @@ namespace admittance_controller {
class AdmittanceParameters : public control_toolbox::ParameterHandler {
public:
AdmittanceParameters() : control_toolbox::ParameterHandler("", 7, 0, 28, 7) {
add_string_parameter("IK.base", false);
add_string_parameter("IK.group_name", false);
add_string_parameter("IK.plugin_name", false);
add_string_parameter("kinematics.base", false);
add_string_parameter("kinematics.group_name", false);
add_string_parameter("kinematics.plugin_name", false);
add_string_parameter("control_frame", true);
add_string_parameter("sensor_frame", false);
add_string_parameter("end_effector_name", false);
Expand Down Expand Up @@ -157,18 +158,18 @@ namespace admittance_controller {
}

void update_storage() override {
ik_base_frame_ = string_parameters_[0].second;
kinematics_base_frame_ = string_parameters_[0].second;
RCUTILS_LOG_INFO_NAMED(
logger_name_.c_str(),
"IK Base frame: %s", ik_base_frame_.c_str());
ik_group_name_ = string_parameters_[1].second;
"IK Base frame: %s", kinematics_base_frame_.c_str());
kinematics_group_name_ = string_parameters_[1].second;
RCUTILS_LOG_INFO_NAMED(
logger_name_.c_str(),
"IK group name frame: %s", ik_group_name_.c_str());
ik_plugin_name_ = string_parameters_[2].second;
"IK group name frame: %s", kinematics_group_name_.c_str());
kinematics_plugin_name_ = string_parameters_[2].second;
RCUTILS_LOG_INFO_NAMED(
logger_name_.c_str(),
"IK plugin name: %s", ik_plugin_name_.c_str());
"IK plugin name: %s", kinematics_plugin_name_.c_str());
control_frame_ = string_parameters_[3].second;
RCUTILS_LOG_INFO_NAMED(
logger_name_.c_str(),
Expand Down Expand Up @@ -235,9 +236,9 @@ namespace admittance_controller {
}

// IK parameters
std::string ik_base_frame_;
std::string ik_group_name_;
std::string ik_plugin_name_;
std::string kinematics_base_frame_;
std::string kinematics_group_name_;
std::string kinematics_plugin_name_;
std::string end_effector_name_;
// Depends on the scenario: usually base_link, tool or end-effector
std::string control_frame_;
Expand All @@ -264,25 +265,8 @@ namespace admittance_controller {
AdmittanceRule() = default;

controller_interface::return_type configure(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, int num_joint);

controller_interface::return_type reset();

// controller_interface::return_type update(
// const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
// const geometry_msgs::msg::Wrench & measured_wrench,
// const geometry_msgs::msg::PoseStamped & reference_pose,
// const rclcpp::Duration & period,
// trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states
// );
//
// controller_interface::return_type update(
// const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
// const geometry_msgs::msg::Wrench & measured_wrench,
// const std::array<double, 6> & reference_joint_deltas,
// const rclcpp::Duration & period,
// trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states
// );

/**
* Calculate 'desired joint states' based on the 'measured force' and 'reference joint state'.
*
Expand All @@ -306,65 +290,70 @@ namespace admittance_controller {

public:
// TODO(destogl): Add parameter for this
bool feedforward_commanded_input_ = true;
bool use_feedforward_commanded_input_ = true;


// Dynamic admittance parameters
AdmittanceParameters parameters_;

// Filter parameter for exponential smoothing
const double alpha = 0.1; // TODO make a ros param
const double EPSILON = 0.001;



protected:
/**
* All values are in he controller frame
* All values are in the controller frame
*/
void calculate_admittance_rule(
destogl marked this conversation as resolved.
Show resolved Hide resolved
const Eigen::Matrix<double,6,1> &wrench,
const rclcpp::Duration &period
const Eigen::Matrix<double,6,1> &desired_vel,
const double dt
);

void process_wrench_measurements(
const geometry_msgs::msg::Wrench &measured_wrench
Eigen::Matrix<double, 6,1> process_wrench_measurements(
const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix<double, 6,1>& last_wrench
);

Eigen::Matrix4d invert_transform(const Eigen::Matrix4d &T);
void normalize_rotation(Eigen::Matrix<double,3,3,Eigen::ColMajor>& R);
Eigen::Matrix<double,4,4,Eigen::ColMajor> invert_transform(Eigen::Matrix<double,4,4,Eigen::ColMajor> &T);
Eigen::Matrix<double,4,4,Eigen::ColMajor> get_transform(const std::vector<double>& positions, const std::string & link_name, bool & success);
Eigen::Vector3d get_rotation_axis(const Eigen::Matrix3d& R) const;
Eigen::Matrix3d normalize_rotation(Eigen::Matrix3d R);
void convert_cartesian_deltas_to_joint_deltas(const std::vector<double>& positions,
const Eigen::Matrix<double, 6,1> & cartesian_delta, std::vector<double>& joint_delta, bool & success);

// Differential IK algorithm (loads a plugin)
std::shared_ptr<pluginlib::ClassLoader<kinematics_interface::KinematicsBaseClass>> ik_loader_;
std::unique_ptr<kinematics_interface::KinematicsBaseClass> ik_;
// Kinematics interface plugin loader
std::shared_ptr<pluginlib::ClassLoader<kinematics_interface::KinematicsBaseClass>> kinematics_loader_;
std::unique_ptr<kinematics_interface::KinematicsBaseClass> kinematics_;

//
// number of robot joint
int num_joints_;

// buffers to pass data to kinematics interface
std::vector<double> transform_buffer_vec;
std::vector<double> joint_buffer_vec;
std::vector<double> cart_buffer_vec;

// admittance controller values
Eigen::Matrix<double,6,1> admittance_acceleration_;
std::vector<double> admittance_acceleration_vec;
Eigen::Matrix<double,6,1> admittance_velocity_;
std::vector<double> admittance_velocity_vec;
Eigen::Matrix<double,4,4,Eigen::ColMajor> admittance_position_;
std::vector<double> admittance_position_vec;

// transforms
Eigen::Matrix<double,4,4,Eigen::ColMajor> cur_ee_transform;
std::vector<double> cur_ee_transform_vec;
Eigen::Matrix<double,4,4,Eigen::ColMajor> desired_ee_transform;
std::vector<double> desired_ee_transform_vec;
Eigen::Matrix<double,4,4,Eigen::ColMajor> reference_ee_transform;
Eigen::Matrix<double,4,4,Eigen::ColMajor> cur_sensor_transform;
std::vector<double> cur_sensor_transform_vec;
Eigen::Matrix<double,4,4,Eigen::ColMajor> cur_control_transform;
std::vector<double> cur_control_transform_vec;
Eigen::Matrix<double,4,4,Eigen::ColMajor> cog_transform;
std::vector<double> cog_transform_vec;

// external force
Eigen::Matrix<double,6,1> wrench;
Eigen::Matrix<double,6,1> desired_ee_vel;
// position of center of gravity in cog_frame
Eigen::Matrix<double,3,1> cog_;
// force applied to sensor due to weight of end effector
Eigen::Matrix<double,3,1> ee_weight;
std::vector<double> desired_ee_vel_vec;

// admittance controller values in joint space
std::vector<double> joint_vel;
std::vector<double> joint_acc;
std::vector<double> joint_pos;
Expand Down
Loading