forked from erdalpekel/franka_ros
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Updating repo with hiro joint velocity and impedance controllers
- Loading branch information
1 parent
86f2254
commit 4461aa5
Showing
15 changed files
with
951 additions
and
3 deletions.
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
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
71 changes: 71 additions & 0 deletions
71
...trollers/include/franka_example_controllers/hiro_cartesian_impedance_example_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,71 @@ | ||
| // Copyright (c) 2017 Franka Emika GmbH | ||
| // Use of this source code is governed by the Apache-2.0 license, see LICENSE | ||
| #pragma once | ||
|
|
||
| #include <memory> | ||
| #include <mutex> | ||
| #include <string> | ||
| #include <vector> | ||
|
|
||
| #include <controller_interface/multi_interface_controller.h> | ||
| #include <dynamic_reconfigure/server.h> | ||
| #include <geometry_msgs/PoseStamped.h> | ||
| #include <hardware_interface/joint_command_interface.h> | ||
| #include <hardware_interface/robot_hw.h> | ||
| #include <ros/node_handle.h> | ||
| #include <ros/time.h> | ||
| #include <Eigen/Dense> | ||
|
|
||
| #include <franka_example_controllers/compliance_paramConfig.h> | ||
| #include <franka_hw/franka_model_interface.h> | ||
| #include <franka_hw/franka_state_interface.h> | ||
|
|
||
| namespace franka_example_controllers { | ||
|
|
||
| class HIROCartesianImpedanceExampleController : public controller_interface::MultiInterfaceController< | ||
| franka_hw::FrankaModelInterface, | ||
| hardware_interface::EffortJointInterface, | ||
| franka_hw::FrankaStateInterface> { | ||
| public: | ||
| bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override; | ||
| void starting(const ros::Time&) override; | ||
| void update(const ros::Time&, const ros::Duration& period) override; | ||
|
|
||
| private: | ||
| // Saturation | ||
| Eigen::Matrix<double, 7, 1> saturateTorqueRate( | ||
| const Eigen::Matrix<double, 7, 1>& tau_d_calculated, | ||
| const Eigen::Matrix<double, 7, 1>& tau_J_d); // NOLINT (readability-identifier-naming) | ||
|
|
||
| std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_; | ||
| std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_; | ||
| std::vector<hardware_interface::JointHandle> joint_handles_; | ||
|
|
||
| double filter_params_{0.005}; | ||
| double nullspace_stiffness_{20.0}; | ||
| double nullspace_stiffness_target_{20.0}; | ||
| const double delta_tau_max_{1.0}; | ||
| Eigen::Matrix<double, 6, 6> cartesian_stiffness_; | ||
| Eigen::Matrix<double, 6, 6> cartesian_stiffness_target_; | ||
| Eigen::Matrix<double, 6, 6> cartesian_damping_; | ||
| Eigen::Matrix<double, 6, 6> cartesian_damping_target_; | ||
| Eigen::Matrix<double, 7, 1> q_d_nullspace_; | ||
| Eigen::Vector3d position_d_; | ||
| Eigen::Quaterniond orientation_d_; | ||
| std::mutex position_and_orientation_d_target_mutex_; | ||
| Eigen::Vector3d position_d_target_; | ||
| Eigen::Quaterniond orientation_d_target_; | ||
|
|
||
| // Dynamic reconfigure | ||
| std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>> | ||
| dynamic_server_compliance_param_; | ||
| ros::NodeHandle dynamic_reconfigure_compliance_param_node_; | ||
| void complianceParamCallback(franka_example_controllers::compliance_paramConfig& config, | ||
| uint32_t level); | ||
|
|
||
| // Equilibrium pose subscriber | ||
| ros::Subscriber sub_equilibrium_pose_; | ||
| void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg); | ||
| }; | ||
|
|
||
| } // namespace franka_example_controllers |
70 changes: 70 additions & 0 deletions
70
..._controllers/include/franka_example_controllers/hiro_joint_impedance_example_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,70 @@ | ||
| // Copyright (c) 2017 Franka Emika GmbH | ||
| // Use of this source code is governed by the Apache-2.0 license, see LICENSE | ||
| #pragma once | ||
|
|
||
| #include <memory> | ||
| #include <string> | ||
| #include <vector> | ||
| #include <mutex> | ||
|
|
||
| #include <controller_interface/multi_interface_controller.h> | ||
| #include <hardware_interface/joint_command_interface.h> | ||
| #include <hardware_interface/robot_hw.h> | ||
| #include <realtime_tools/realtime_publisher.h> | ||
| #include <ros/node_handle.h> | ||
| #include <ros/time.h> | ||
|
|
||
| #include <franka_example_controllers/JointTorqueComparison.h> | ||
| #include <franka_hw/franka_cartesian_command_interface.h> | ||
| #include <franka_hw/franka_model_interface.h> | ||
| #include <franka_hw/trigger_rate.h> | ||
| #include <sensor_msgs/JointState.h> | ||
| #include <std_msgs/Float32MultiArray.h> | ||
|
|
||
| namespace franka_example_controllers { | ||
|
|
||
| class HIROJointImpedanceExampleController : public controller_interface::MultiInterfaceController< | ||
| franka_hw::FrankaModelInterface, | ||
| hardware_interface::EffortJointInterface, | ||
| franka_hw::FrankaPoseCartesianInterface> { | ||
| public: | ||
| bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override; | ||
| void starting(const ros::Time&) override; | ||
| void update(const ros::Time&, const ros::Duration& period) override; | ||
|
|
||
| private: | ||
| // Saturation | ||
| std::array<double, 7> saturateTorqueRate( | ||
| const std::array<double, 7>& tau_d_calculated, | ||
| const std::array<double, 7>& tau_J_d); // NOLINT (readability-identifier-naming) | ||
|
|
||
| std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_; | ||
| std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_; | ||
| std::vector<hardware_interface::JointHandle> joint_handles_; | ||
|
|
||
| static constexpr double kDeltaTauMax{1.0}; | ||
| double radius_{0.1}; | ||
| double acceleration_time_{2.0}; | ||
| double vel_max_{0.05}; | ||
| double angle_{0.0}; | ||
| double vel_current_{0.0}; | ||
|
|
||
| std::vector<double> k_gains_; | ||
| std::vector<double> d_gains_; | ||
| double coriolis_factor_{1.0}; | ||
| std::array<double, 7> dq_filtered_; | ||
| std::array<double, 16> initial_pose_; | ||
|
|
||
| franka_hw::TriggerRate rate_trigger_{1.0}; | ||
| std::array<double, 7> last_tau_d_{}; | ||
| realtime_tools::RealtimePublisher<JointTorqueComparison> torques_publisher_; | ||
| // Added by Caleb E. | ||
| std::mutex joint_position_and_velocity_d_target_mutex_; | ||
| std::vector<double> joint_positions_{0,0,0,0,0,0,0}; | ||
| bool callback_done_once = false; | ||
| // void jointCommandCb(const sensor_msgs::JointState::ConstPtr& joint_pos_commands); | ||
| void jointCommandCb(const std_msgs::Float32MultiArray::ConstPtr& joint_pos_commands); | ||
| ros::Subscriber sub_command_; | ||
| }; | ||
|
|
||
| } // namespace franka_example_controllers |
47 changes: 47 additions & 0 deletions
47
...e_controllers/include/franka_example_controllers/hiro_joint_velocity_example_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,47 @@ | ||
| // Copyright (c) 2017 Franka Emika GmbH | ||
| // Use of this source code is governed by the Apache-2.0 license, see LICENSE | ||
| #pragma once | ||
|
|
||
| #include <string> | ||
| #include <vector> | ||
|
|
||
| #include <controller_interface/multi_interface_controller.h> | ||
| #include <franka_hw/franka_state_interface.h> | ||
| #include <hardware_interface/joint_command_interface.h> | ||
| #include <hardware_interface/robot_hw.h> | ||
| #include <ros/node_handle.h> | ||
| #include <ros/time.h> | ||
| #include <sensor_msgs/JointState.h> | ||
|
|
||
| namespace franka_example_controllers { | ||
|
|
||
| class HIROJointVelocityExampleController : public controller_interface::MultiInterfaceController< | ||
| hardware_interface::VelocityJointInterface, | ||
| franka_hw::FrankaStateInterface> { | ||
| public: | ||
| bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override; | ||
| void update(const ros::Time&, const ros::Duration& period) override; | ||
| void starting(const ros::Time&) override; | ||
| void stopping(const ros::Time&) override; | ||
|
|
||
| private: | ||
| hardware_interface::VelocityJointInterface* velocity_joint_interface_; | ||
| std::vector<hardware_interface::JointHandle> velocity_joint_handles_; | ||
| ros::Duration elapsed_time_; | ||
| bool first_sample_taken; | ||
| void jointCommandCb(const sensor_msgs::JointState::ConstPtr& joint_pos_commands); | ||
| ros::Subscriber sub_command_; | ||
| std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_; | ||
| int last_time_called; | ||
| // std::array<double, 7> joint_velocities{}; | ||
| std::vector<double> joint_positions_{0,0,0,0,0,0,0}; | ||
| float p_ = 1.9; | ||
| float d_ = 0.0; | ||
| // float curr_error_; | ||
| std::vector<double> prev_error_{0,0,0,0,0,0,0}; | ||
| std::vector<double> curr_error_{0,0,0,0,0,0,0}; | ||
| bool callback_done_once = false; | ||
| // franka::RobotState robot_state | ||
| }; | ||
|
|
||
| } // namespace franka_example_controllers |
14 changes: 14 additions & 0 deletions
14
franka_example_controllers/launch/hiro_cartesian_impedance_example_controller.launch
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,14 @@ | ||
| <?xml version="1.0" ?> | ||
| <launch> | ||
| <arg name="robot" default="fr3" doc="choose your robot. Possible values: [panda, fr3]"/> | ||
| <arg name="arm_id" default="$(arg robot)" /> | ||
| <include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true"/> | ||
| <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" subst_value="true" /> | ||
| <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="hiro_cartesian_impedance_example_controller"/> | ||
| <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_description_with_marker.rviz -f $(arg arm_id)_link0 --splash-screen $(find franka_visualization)/splash.png"/> | ||
| <node name="interactive_marker" pkg="franka_example_controllers" type="interactive_marker.py" required="true" output="screen"> | ||
| <param name="link_name" value="$(arg arm_id)_link0" /> | ||
| <remap from="equilibrium_pose" to="/hiro_cartesian_impedance_example_controller/equilibrium_pose" /> | ||
| </node> | ||
| <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" /> | ||
| </launch> |
9 changes: 9 additions & 0 deletions
9
franka_example_controllers/launch/hiro_joint_impedance_example_controller.launch
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,9 @@ | ||
| <?xml version="1.0" ?> | ||
| <launch> | ||
| <arg name="robot" default="fr3" doc="choose your robot. Possible values: [panda, fr3]"/> | ||
| <arg name="arm_id" default="$(arg robot)" /> | ||
| <include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true"/> | ||
| <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" subst_value="true" /> | ||
| <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="hiro_joint_impedance_example_controller"/> | ||
| <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz -f $(arg arm_id)_link0 --splash-screen $(find franka_visualization)/splash.png"/> | ||
| </launch> |
10 changes: 10 additions & 0 deletions
10
franka_example_controllers/launch/hiro_joint_velocity_example_controller.launch
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,10 @@ | ||
| <?xml version="1.0" ?> | ||
| <launch> | ||
| <arg name="robot_ip" default="192.168.0.198"/> | ||
| <arg name="robot" default="fr3" doc="choose your robot. Possible values: [panda, fr3]"/> | ||
| <arg name="arm_id" default="$(arg robot)" /> | ||
| <!-- <arg name="load_gripper" default="true" /> --> | ||
| <include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true" /> | ||
| <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" subst_value="true" /> | ||
| <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="hiro_joint_velocity_example_controller"/> <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/skin_demo.rviz -f $(arg arm_id)_link0 splash-screen $(find franka_visualization)/splash.png"/> | ||
| </launch> |
2 changes: 1 addition & 1 deletion
2
franka_example_controllers/launch/joint_velocity_example_controller.launch
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
Oops, something went wrong.