dual_arm_cartesian_impedance_example_controller.h
Go to the documentation of this file.
1 // Copyright (c) 2019 Franka Emika GmbH
2 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
3 #pragma once
4 
5 #include <memory>
6 #include <string>
7 #include <vector>
8 
10 #include <dynamic_reconfigure/server.h>
11 #include <geometry_msgs/PoseStamped.h>
15 #include <ros/node_handle.h>
16 #include <ros/time.h>
17 #include <Eigen/Dense>
18 
19 #include <franka_example_controllers/dual_arm_compliance_paramConfig.h>
22 #include <franka_hw/trigger_rate.h>
23 
25 
31  std::unique_ptr<franka_hw::FrankaStateHandle>
33  std::unique_ptr<franka_hw::FrankaModelHandle>
35  std::vector<hardware_interface::JointHandle> joint_handles_;
36  double filter_params_{0.005};
37  double nullspace_stiffness_{20.0};
40  double nullspace_stiffness_target_{20.0};
42  const double delta_tau_max_{1.0};
43  Eigen::Matrix<double, 6, 6> cartesian_stiffness_;
46  Eigen::Matrix<double, 6, 6> cartesian_stiffness_target_;
47  Eigen::Matrix<double, 6, 6> cartesian_damping_;
48  Eigen::Matrix<double, 6, 6> cartesian_damping_target_;
49  Eigen::Matrix<double, 7, 1> q_d_nullspace_;
50  Eigen::Vector3d position_d_;
53  Eigen::Quaterniond orientation_d_;
54  Eigen::Vector3d position_d_target_;
55  Eigen::Quaterniond orientation_d_target_;
56 };
57 
65  franka_hw::FrankaModelInterface,
66  hardware_interface::EffortJointInterface,
67  franka_hw::FrankaStateInterface> {
68  public:
77  bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;
78 
83  void starting(const ros::Time&) override;
84 
90  void update(const ros::Time&, const ros::Duration& period) override;
91 
92  private:
93  std::map<std::string, FrankaDataContainer>
95  std::string left_arm_id_;
96  std::string right_arm_id_;
97 
99  Eigen::Affine3d Ol_T_Or_; // NOLINT (readability-identifier-naming)
101  Eigen::Affine3d EEr_T_EEl_; // NOLINT (readability-identifier-naming)
103  Eigen::Affine3d EEl_T_C_;
104 
109 
118  Eigen::Matrix<double, 7, 1> saturateTorqueRate(
119  const FrankaDataContainer& arm_data,
120  const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
121  const Eigen::Matrix<double, 7, 1>& tau_J_d); // NOLINT (readability-identifier-naming)
122 
131  bool initArm(hardware_interface::RobotHW* robot_hw,
132  const std::string& arm_id,
133  const std::vector<std::string>& joint_names);
134 
140  void updateArm(FrankaDataContainer& arm_data);
141 
147  void startingArm(FrankaDataContainer& arm_data);
148 
150  std::unique_ptr<dynamic_reconfigure::Server<
151  franka_combined_example_controllers::dual_arm_compliance_paramConfig>>
153 
156 
162  void complianceParamCallback(
163  franka_combined_example_controllers::dual_arm_compliance_paramConfig& config,
164  uint32_t /*level*/);
165 
168 
174  void targetPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
175 
179  void publishCenteringPose();
180 };
181 
182 } // namespace franka_example_controllers
Eigen::Matrix< double, 6, 6 > cartesian_damping_target_
Unfiltered raw value.
std::unique_ptr< dynamic_reconfigure::Server< franka_combined_example_controllers::dual_arm_compliance_paramConfig > > dynamic_server_compliance_param_
Nodehandle for the dynamic reconfigure namespace.
Eigen::Affine3d EEl_T_C_
< Transformation from the centering frame to the left end effector.
Eigen::Matrix< double, 6, 6 > cartesian_stiffness_
To track the target pose.
Eigen::Matrix< double, 6, 6 > cartesian_damping_
To damp cartesian motions.
Eigen::Affine3d EEr_T_EEl_
< Target transformation between the two endeffectors.
Eigen::Matrix< double, 7, 1 > q_d_nullspace_
Target joint pose for nullspace motion.
std::unique_ptr< franka_hw::FrankaStateHandle > state_handle_
To read to complete robot state.
std::vector< hardware_interface::JointHandle > joint_handles_
To command joint torques.
std::unique_ptr< franka_hw::FrankaModelHandle > model_handle_
To have access to e.g. jacobians.
realtime_tools::RealtimePublisher< geometry_msgs::PoseStamped > center_frame_pub_
Rate to trigger publishing the current pose of the centering frame.
std::string left_arm_id_
Name of the left arm, retrieved from the parameter server.
std::map< std::string, FrankaDataContainer > arms_data_
Holds all relevant data for both arms.
double nullspace_stiffness_
[Nm/rad] To track the initial joint configuration in the nullspace of the Cartesian motion...
Eigen::Quaterniond orientation_d_
Target orientation of the end effector.
Controller class for ros_control that renders two decoupled Cartesian impedances for the tracking of ...
double filter_params_
[-] PT1-Filter constant to smooth target values set by dynamic reconfigure servers (stiffness/damping...
This container holds all data and parameters used to control one panda arm with a Cartesian impedance...
Eigen::Matrix< double, 6, 6 > cartesian_stiffness_target_
Unfiltered raw value.
void update(controller_manager::ControllerManager &cm, const ros::TimerEvent &e)
Eigen::Vector3d position_d_
Target position of the end effector.
const double delta_tau_max_
[Nm/ms] Maximum difference in joint-torque per timestep.
std::string right_arm_id_
Name of the right arm, retrieved from the parameter server.


franka_example_controllers
Author(s): Franka Emika GmbH
autogenerated on Fri Oct 23 2020 03:47:17