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.

Sign up for GitHub

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

Added SK scripts and launch #2

Merged
merged 1 commit into from
Nov 18, 2024
Merged

Added SK scripts and launch #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
16 changes: 12 additions & 4 deletions franka_control/config/franka_control_node.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,11 +20,19 @@ internal_controller: joint_impedance
realtime_config: enforce
# Configure the initial defaults for the collision behavior reflexes.
collision_config:
# lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
# upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
# lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
# upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
# lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
# upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
# lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
# upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
upper_torque_thresholds_acceleration: [200.0, 200.0, 180.0, 180.0, 160.0, 140.0, 120.0] # [Nm]
lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
upper_torque_thresholds_nominal: [200.0, 200.0, 180.0, 180.0, 160.0, 140.0, 120.0] # [Nm]
lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
upper_force_thresholds_acceleration: [200.0, 200.0, 200.0, 250.0, 250.0, 250.0] # [N, N, N, Nm, Nm, Nm]
lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
upper_force_thresholds_nominal: [200.0, 200.0, 200.0, 250.0, 250.0, 250.0] # [N, N, N, Nm, Nm, Nm]
2 changes: 1 addition & 1 deletion franka_control/launch/franka_control.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<?xml version="1.0" ?>
<launch>
<arg name="robot_ip" />
<arg name="robot_ip" default="192.168.0.100" />
<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" />
Expand Down
56 changes: 42 additions & 14 deletions franka_example_controllers/config/franka_example_controllers.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,21 +73,49 @@ hiro_joint_impedance_example_controller:
- $(arg arm_id)_joint6
- $(arg arm_id)_joint7
k_gains:
- 10.0
- 10.0
- 10.0
- 10.0
- 5.0
- 3.0
- 1.0
- 80.0
- 80.0
- 80.0
- 80.0
- 40.0
- 24.0
- 8.0
# - 20.0
# - 20.0
# - 20.0
# - 20.0
# - 10.0
# - 6.0
# - 2.0
# - 0.0
# - 0.0
# - 0.0
# - 0.0
# - 0.0
# - 0.0
# - 0.0
d_gains:
- 5.0
- 5.0
- 5.0
- 2.0
- 2.0
- 2.0
- 1.0
- 40.0
- 40.0
- 40.0
- 16.0
- 16.0
- 16.0
- 8.0
# - 10.0
# - 10.0
# - 10.0
# - 4.0
# - 4.0
# - 4.0
# - 2.0
# - 0.0
# - 0.0
# - 0.0
# - 0.0
# - 0.0
# - 0.0
# - 0.0
radius: 0.1
acceleration_time: 2.0
vel_max: 0.15
Expand Down
21 changes: 18 additions & 3 deletions ..._controllers/include/franka_example_controllers/hiro_joint_impedance_example_controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <franka_hw/trigger_rate.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Bool.h>

namespace franka_example_controllers {

Expand Down Expand Up @@ -49,11 +50,9 @@ class HIROJointImpedanceExampleController : public controller_interface::MultiIn
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_;
std::array<double, 16> pose_from_cb_;

franka_hw::TriggerRate rate_trigger_{1.0};
std::array<double, 7> last_tau_d_{};
Expand All @@ -64,8 +63,24 @@ class HIROJointImpedanceExampleController : public controller_interface::MultiIn
bool callback_done_once = false;
void xboxCommandCb(const sensor_msgs::JointState::ConstPtr& joint_pos_commands);
void cuRoboCommandCb(const std_msgs::Float32MultiArray::ConstPtr& joint_pos_commands);
void impedanceChangeBoolCb(const std_msgs::Bool::ConstPtr& impedance_change_bool);
ros::Subscriber sub_command_;
ros::Subscriber sub_command2_;
ros::Subscriber sub_impedance_change_bool_;
// std::vector<double> non_zero_imp_k{600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0};
// std::vector<double> non_zero_imp_d{50.0, 50.0, 50.0, 20.0, 20.0, 20.0, 10.0};
// std::vector<double> k_gains_{160.0, 160.0, 160.0, 160.0, 80.0, 48.0, 16.0};
// std::vector<double> d_gains_{40.0, 40.0, 40.0, 16.0, 16.0, 16.0, 8.0};
std::vector<double> k_gains_{80.0, 80.0, 80.0, 80.0, 40.0, 24.0, 8.0};
std::vector<double> d_gains_{40.0, 40.0, 40.0, 16.0, 16.0, 16.0, 8.0};
// std::vector<double> k_gains_{40.0, 40.0, 40.0, 40.0, 20.0, 12.0, 4.0};
// std::vector<double> d_gains_{20.0, 20.0, 20.0, 8.0, 8.0, 8.0, 4.0};
// std::vector<double> non_zero_imp_k{160.0, 160.0, 160.0, 160.0, 80.0, 48.0, 16.0};
// std::vector<double> non_zero_imp_d{40.0, 40.0, 40.0, 16.0, 16.0, 16.0, 8.0};
std::vector<double> non_zero_imp_k{80.0, 80.0, 80.0, 80.0, 40.0, 24.0, 8.0};
std::vector<double> non_zero_imp_d{40.0, 40.0, 40.0, 16.0, 16.0, 16.0, 8.0};
// std::vector<double> non_zero_imp_k{40.0, 40.0, 40.0, 40.0, 20.0, 12.0, 4.0};
// std::vector<double> non_zero_imp_d{20.0, 20.0, 20.0, 8.0, 8.0, 8.0, 4.0};
};

} // namespace franka_example_controllers
2 changes: 1 addition & 1 deletion franka_example_controllers/launch/force_example_controller.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<?xml version="1.0" ?>
<launch>
<arg name="robot" default="panda" doc="choose your robot. Possible values: [panda, fr3]"/>
<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" />
Expand Down
10 changes: 8 additions & 2 deletions franka_example_controllers/launch/hiro_joint_impedance_example_controller.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,15 @@
<launch>
<arg name="robot" default="fr3" doc="choose your robot. Possible values: [panda, fr3]"/>
<arg name="arm_id" default="$(arg robot)" />
<arg name="robot_ip" default="192.168.0.198"/>
<arg name="robot_ip" default="192.168.0.100"/>
<!-- <arg name="robot_ip" default="192.168.1.3"/> -->
<arg name="launch_rviz" default="false"/>
<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"/>

<group if="$(arg launch_rviz)">
<!-- stuff that will only be evaluated if foo is true -->
<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"/>
</group>
</launch>
2 changes: 1 addition & 1 deletion franka_example_controllers/launch/joint_impedance_example_controller.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<?xml version="1.0" ?>
<launch>
<arg name="robot" default="panda" doc="choose your robot. Possible values: [panda, fr3]"/>
<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" />
Expand Down
Loading
Loading