From 4cfb84f3f5f11accdab8d0d58fbf9e39c223b3a0 Mon Sep 17 00:00:00 2001 From: CalebEscobedo Date: Mon, 18 Nov 2024 12:08:37 -0700 Subject: [PATCH] Added SK scripts and launch --- .../config/franka_control_node.yaml | 16 +- franka_control/launch/franka_control.launch | 2 +- .../config/franka_example_controllers.yaml | 56 ++++-- .../hiro_joint_impedance_example_controller.h | 21 +- .../launch/force_example_controller.launch | 2 +- ..._joint_impedance_example_controller.launch | 10 +- .../joint_impedance_example_controller.launch | 2 +- franka_example_controllers/launch/robot.rviz | 180 ++++++++++++++---- .../rviz/franka_description_with_marker.rviz | 91 ++++++++- .../scripts/move_to_start.py | 127 +++++++++++- ...iro_joint_impedance_example_controller.cpp | 68 ++++--- 11 files changed, 475 insertions(+), 100 deletions(-) diff --git a/franka_control/config/franka_control_node.yaml b/franka_control/config/franka_control_node.yaml index 4ceb6fee6..51751da3c 100644 --- a/franka_control/config/franka_control_node.yaml +++ b/franka_control/config/franka_control_node.yaml @@ -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] diff --git a/franka_control/launch/franka_control.launch b/franka_control/launch/franka_control.launch index b7c001767..dcc738b98 100644 --- a/franka_control/launch/franka_control.launch +++ b/franka_control/launch/franka_control.launch @@ -1,6 +1,6 @@ - + diff --git a/franka_example_controllers/config/franka_example_controllers.yaml b/franka_example_controllers/config/franka_example_controllers.yaml index 068c2c983..e3436e29e 100644 --- a/franka_example_controllers/config/franka_example_controllers.yaml +++ b/franka_example_controllers/config/franka_example_controllers.yaml @@ -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 diff --git a/franka_example_controllers/include/franka_example_controllers/hiro_joint_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/hiro_joint_impedance_example_controller.h index 789695346..48e3262c5 100644 --- a/franka_example_controllers/include/franka_example_controllers/hiro_joint_impedance_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/hiro_joint_impedance_example_controller.h @@ -20,6 +20,7 @@ #include #include #include +#include namespace franka_example_controllers { @@ -49,11 +50,9 @@ class HIROJointImpedanceExampleController : public controller_interface::MultiIn double angle_{0.0}; double vel_current_{0.0}; - std::vector k_gains_; - std::vector d_gains_; double coriolis_factor_{1.0}; std::array dq_filtered_; - std::array initial_pose_; + std::array pose_from_cb_; franka_hw::TriggerRate rate_trigger_{1.0}; std::array last_tau_d_{}; @@ -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 non_zero_imp_k{600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0}; + // std::vector non_zero_imp_d{50.0, 50.0, 50.0, 20.0, 20.0, 20.0, 10.0}; + // std::vector k_gains_{160.0, 160.0, 160.0, 160.0, 80.0, 48.0, 16.0}; + // std::vector d_gains_{40.0, 40.0, 40.0, 16.0, 16.0, 16.0, 8.0}; + std::vector k_gains_{80.0, 80.0, 80.0, 80.0, 40.0, 24.0, 8.0}; + std::vector d_gains_{40.0, 40.0, 40.0, 16.0, 16.0, 16.0, 8.0}; + // std::vector k_gains_{40.0, 40.0, 40.0, 40.0, 20.0, 12.0, 4.0}; + // std::vector d_gains_{20.0, 20.0, 20.0, 8.0, 8.0, 8.0, 4.0}; + // std::vector non_zero_imp_k{160.0, 160.0, 160.0, 160.0, 80.0, 48.0, 16.0}; + // std::vector non_zero_imp_d{40.0, 40.0, 40.0, 16.0, 16.0, 16.0, 8.0}; + std::vector non_zero_imp_k{80.0, 80.0, 80.0, 80.0, 40.0, 24.0, 8.0}; + std::vector non_zero_imp_d{40.0, 40.0, 40.0, 16.0, 16.0, 16.0, 8.0}; + // std::vector non_zero_imp_k{40.0, 40.0, 40.0, 40.0, 20.0, 12.0, 4.0}; + // std::vector non_zero_imp_d{20.0, 20.0, 20.0, 8.0, 8.0, 8.0, 4.0}; }; } // namespace franka_example_controllers diff --git a/franka_example_controllers/launch/force_example_controller.launch b/franka_example_controllers/launch/force_example_controller.launch index 13ebdd014..fe31f07a6 100644 --- a/franka_example_controllers/launch/force_example_controller.launch +++ b/franka_example_controllers/launch/force_example_controller.launch @@ -1,6 +1,6 @@ - + diff --git a/franka_example_controllers/launch/hiro_joint_impedance_example_controller.launch b/franka_example_controllers/launch/hiro_joint_impedance_example_controller.launch index 105e3316d..9de24df19 100644 --- a/franka_example_controllers/launch/hiro_joint_impedance_example_controller.launch +++ b/franka_example_controllers/launch/hiro_joint_impedance_example_controller.launch @@ -2,9 +2,15 @@ - + + + - + + + + + diff --git a/franka_example_controllers/launch/joint_impedance_example_controller.launch b/franka_example_controllers/launch/joint_impedance_example_controller.launch index d418ad7cd..069874f0d 100644 --- a/franka_example_controllers/launch/joint_impedance_example_controller.launch +++ b/franka_example_controllers/launch/joint_impedance_example_controller.launch @@ -1,6 +1,6 @@ - + diff --git a/franka_example_controllers/launch/robot.rviz b/franka_example_controllers/launch/robot.rviz index 07c4f7ffe..0cda95ed5 100644 --- a/franka_example_controllers/launch/robot.rviz +++ b/franka_example_controllers/launch/robot.rviz @@ -1,13 +1,15 @@ Panels: - Class: rviz/Displays - Help Height: 89 + Help Height: 138 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 + - /MarkerArray1 + - /OverlayText1 Splitter Ratio: 0.5 - Tree Height: 746 + Tree Height: 627 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -16,17 +18,21 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 + - /Orbit1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -36,7 +42,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -58,61 +64,110 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - panda_hand: + fr3_hand: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_leftfinger: + fr3_hand_sc: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_link0: + fr3_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + fr3_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr3_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr3_link0_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr3_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr3_link1_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr3_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr3_link2_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr3_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr3_link3_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr3_link4: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_link1: + fr3_link4_sc: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_link2: + fr3_link5: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_link3: + fr3_link5_sc: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_link4: + fr3_link6: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_link5: + fr3_link6_sc: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_link6: + fr3_link7: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_link7: + fr3_link7_sc: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_link8: + fr3_link8: Alpha: 1 Show Axes: false Show Trail: false - panda_rightfinger: + fr3_rightfinger: Alpha: 1 Show Axes: false Show Trail: false @@ -123,10 +178,40 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /marker_list + Name: MarkerArray + Namespaces: + "": true + Queue Size: 100 + Value: true + - Align Bottom: false + Background Alpha: 0.800000011920929 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.800000011920929 + Foreground Color: 25; 255; 240 + Invert Shadow: false + Name: OverlayText + Overtake BG Color Properties: false + Overtake FG Color Properties: false + Overtake Position Properties: false + Topic: /text_overlay + Value: true + font: DejaVu Sans Mono + height: 128 + left: 0 + line width: 2 + text size: 12 + top: 0 + width: 128 Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: panda_link0 + Default Light: true + Fixed Frame: fr3_link0 Frame Rate: 30 Name: root Tools: @@ -137,7 +222,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -147,33 +235,53 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 3.39362764 + Distance: 2.8389763832092285 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: - X: -0.139062241 - Y: 0.151414081 - Z: 0.0437288694 + X: -0.09346241503953934 + Y: -0.09422435611486435 + Z: 0.3792321979999542 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.485397696 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.2403983622789383 Target Frame: - Value: Orbit (rviz) - Yaw: 0.500398219 - Saved: ~ + Yaw: 0.010398158803582191 + Saved: + - Class: rviz/Orbit + Distance: 2.5528829097747803 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -0.09346241503953934 + Y: -0.09422435611486435 + Z: 0.3792321979999542 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Orbit + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3803982436656952 + Target Frame: + Yaw: 0.7053978443145752 Window Geometry: Displays: - collapsed: false - Height: 1059 - Hide Left Dock: false + collapsed: true + Height: 1016 + Hide Left Dock: true Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000384fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000384000000f300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000384fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004100000384000000bb00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e0000027600fffffffb0000000800540069006d00650100000000000004500000000000000000000004f90000038400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001f70000033afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000033a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000006eafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000006ea000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d0065000000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000738000003c500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -182,6 +290,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1918 - X: 1920 - Y: 19 + Width: 1848 + X: 72 + Y: 27 diff --git a/franka_example_controllers/launch/rviz/franka_description_with_marker.rviz b/franka_example_controllers/launch/rviz/franka_description_with_marker.rviz index eacde2590..8be2f0619 100644 --- a/franka_example_controllers/launch/rviz/franka_description_with_marker.rviz +++ b/franka_example_controllers/launch/rviz/franka_description_with_marker.rviz @@ -7,7 +7,7 @@ Panels: - /Global Options1 - /Status1 Splitter Ratio: 0.5 - Tree Height: 573 + Tree Height: 697 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -23,10 +23,13 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -58,50 +61,114 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link0: Alpha: 1 Show Axes: false Show Trail: false Value: true + panda_link0_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link1: Alpha: 1 Show Axes: false Show Trail: false Value: true + panda_link1_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link2: Alpha: 1 Show Axes: false Show Trail: false Value: true + panda_link2_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link3: Alpha: 1 Show Axes: false Show Trail: false Value: true + panda_link3_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link4: Alpha: 1 Show Axes: false Show Trail: false Value: true + panda_link4_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link5: Alpha: 1 Show Axes: false Show Trail: false Value: true + panda_link5_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link6: Alpha: 1 Show Axes: false Show Trail: false Value: true + panda_link6_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link7: Alpha: 1 Show Axes: false Show Trail: false Value: true + panda_link7_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link8: Alpha: 1 Show Axes: false Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Name: RobotModel Robot Description: robot_description TF Prefix: "" @@ -120,6 +187,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: panda_link0 Frame Rate: 30 Name: root @@ -131,7 +199,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -141,12 +212,13 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 4.644041061401367 + Distance: 2.7850098609924316 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: X: 0 Y: 0 @@ -156,18 +228,17 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.785398006439209 + Pitch: 0.5903980731964111 Target Frame: - Value: Orbit (rviz) - Yaw: 0.785398006439209 + Yaw: 0.7353980541229248 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 846 + Height: 998 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000027000002c5000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000027000002c50000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000042fc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002c500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000344fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000344000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000344fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000344000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006b300000042fc0100000002fb0000000800540069006d00650100000000000006b3000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000042e0000034400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -176,6 +247,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1200 - X: 295 + Width: 1715 + X: 85 Y: 39 diff --git a/franka_example_controllers/scripts/move_to_start.py b/franka_example_controllers/scripts/move_to_start.py index 0e75e3dde..bfe58be7e 100755 --- a/franka_example_controllers/scripts/move_to_start.py +++ b/franka_example_controllers/scripts/move_to_start.py @@ -1,5 +1,4 @@ #!/usr/bin/env python - import sys import rospy as ros @@ -39,10 +38,12 @@ goal.trajectory.joint_names, point.positions = [list(x) for x in zip(*pose.items())] point.velocities = [0] * len(pose) - +print(point.positions) goal.trajectory.points.append(point) goal.goal_time_tolerance = ros.Duration.from_sec(0.5) +print(goal) + ros.loginfo('Sending trajectory Goal to move into initial config') client.send_goal_and_wait(goal) @@ -76,3 +77,125 @@ else: ros.loginfo('move_to_start: Successfully moved into start pose') + + + +''' +#!/usr/bin/env python +import time +import sys +import rospy as ros +import numpy as np +from actionlib import SimpleActionClient +from sensor_msgs.msg import JointState +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from control_msgs.msg import FollowJointTrajectoryAction, \ + FollowJointTrajectoryGoal, FollowJointTrajectoryResult + +ros.init_node('move_to_start') + +file_path = '/home/caleb/ros_relaxed_ik_ws/src/relaxed_ik_ros1/scripts/shy.npz' +data = np.load(file_path) +print(data['q'],data['qdot']) +print(len(data['q']), len(data['qdot']), "C") +q = [[0,0,0,0,0,0,0]]*int(len(data['q'])) +qdot = [[0,0,0,0,0,0,0]]*int(len(data['qdot'])) + +action = ros.resolve_name('~follow_joint_trajectory') +client = SimpleActionClient(action, FollowJointTrajectoryAction) +ros.loginfo("move_to_start: Waiting for '" + action + "' action to come up") +client.wait_for_server() + +param = ros.resolve_name('~joint_pose') +pose = ros.get_param(param, None) +#if pose is None: +# ros.logerr('move_to_start: Could not find required parameter "' + param + '"') +# sys.exit(1) + +#topic = ros.resolve_name('~joint_states') +#ros.loginfo("move_to_start: Waiting for message on topic '" + topic + "'") +#joint_state = ros.wait_for_message(topic, JointState) +#initial_pose = dict(zip(joint_state.name, joint_state.position)) + +#max_movement = max(abs(pose[joint] - initial_pose[joint]) for joint in pose) +points = [] +for index in range(0,len(data['q'])): + print(len(data['q'])) + point = JointTrajectoryPoint() + point.time_from_start = ros.Duration.from_sec( + (index+1)*0.033 + ) + #point.time_from_start = ros.Duration.from_sec( + # # Use either the time to move the furthest joint with 'max_dq' or 500ms, + # # whatever is greater + # max(max_movement / ros.get_param('~max_dq', 0.5), 0.5) + #) + #goal = FollowJointTrajectoryGoal() + #goal.trajectory.joint_names, point.positions = [list(x) for x in zip(*pose.items())] + #point.velocities = [0] * len(pose) + #print(len(point.positions), type(point.positions),"A") + #goal.trajectory.points.append(point) + #goal.goal_time_tolerance = ros.Duration.from_sec(0.5) + + #qdot = [[0,0,0,0,0,0,0]]*int(len(data['qdot'])) + #q = [[0,0,0,0,0,0,0]]*10 + #qdot = [[0,0,0,0,0,0,0]]*10 + #q[0] = [0, -0.785398163397, 0, -2.35619449019, 0, 1.57079632679, 0.785398163397] + #qdot[0] = [0, 0, 0, 0, 0, 0, 0] + #for i in range(0,7): + # q[index][i] = data['q'][index][i] + # qdot[index][i] = data['qdot'][index][i] + q = data['q'] + qdot = data['qdot'] + #for i in range(1,10): + # qdot[i] = [0+i*0.1, 0+i*0.1, 0+i*0.1, 0+i*0.1, 0+i*0.1, 0+i*0.1, 0+i*0.1] + # q[i] = [q[i-1][0]+qdot[i][0], q[i-1][1]+qdot[i][1], q[i-1][2]+qdot[i][2], q[i-1][3]+qdot[i][3], q[i-1][4]+qdot[i][4], q[i-1][5]+qdot[i][5], q[i-1][6]+qdot[i][6]] + + ros.loginfo('Sending trajectory Goal to move into initial config') + #for joint_angle in range(0, len(q)): + #index = joint_angle + point.positions = q[index] + point.velocities = [0,0,0,0,0,0,0]#qdot[index] + points.append(point) + +#print(len(point.positions), type(point.positions),"B") +goal = FollowJointTrajectoryGoal() +goal.trajectory.joint_names, a = [list(x) for x in zip(*pose.items())] +print(goal.trajectory.joint_names) +#point.velocities = [0] * 7 +#point.positions = [0] * len(pose) +goal.trajectory.points = points +#goal.trajectory.points.append(point) +print(len(goal.trajectory.points),goal.trajectory.points[0],goal.trajectory.points[1],"AAAAAAAAAAAAAAAAAAA") +client.send_goal_and_wait(goal) +result = client.get_result() +if result.error_code != FollowJointTrajectoryResult.SUCCESSFUL: + ros.logerr('move_to_start: Movement was not successful: ' + { + FollowJointTrajectoryResult.INVALID_GOAL: + """ + The joint pose you want to move to is invalid (e.g. unreachable, singularity...). + Is the 'joint_pose' reachable? + """, + + FollowJointTrajectoryResult.INVALID_JOINTS: + """ + The joint pose you specified is for different joints than the joint trajectory controller + is claiming. Does you 'joint_pose' include all 7 joints of the robot? + """, + + FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED: + """ + During the motion the robot deviated from the planned path too much. Is something blocking + the robot? + """, + + FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED: + """ + After the motion the robot deviated from the desired goal pose too much. Probably the robot + didn't reach the joint_pose properly + """, + }[result.error_code]) + +else: + ros.loginfo('move_to_start: Successfully moved into start pose') +''' \ No newline at end of file diff --git a/franka_example_controllers/src/hiro_joint_impedance_example_controller.cpp b/franka_example_controllers/src/hiro_joint_impedance_example_controller.cpp index f24a94171..b9cba44e7 100644 --- a/franka_example_controllers/src/hiro_joint_impedance_example_controller.cpp +++ b/franka_example_controllers/src/hiro_joint_impedance_example_controller.cpp @@ -19,6 +19,9 @@ bool HIROJointImpedanceExampleController::init(hardware_interface::RobotHW* robo /* CB function for curobo*/ sub_command_ = node_handle.subscribe( "/cu_joint_solution", 1, &HIROJointImpedanceExampleController::cuRoboCommandCb, this); + + sub_impedance_change_bool_ = node_handle.subscribe( + "/impedance_change_bool", 1, &HIROJointImpedanceExampleController::impedanceChangeBoolCb, this); /* CB function for xbox_input file*/ sub_command2_ = node_handle.subscribe( @@ -55,20 +58,6 @@ bool HIROJointImpedanceExampleController::init(hardware_interface::RobotHW* robo return false; } - if (!node_handle.getParam("k_gains", k_gains_) || k_gains_.size() != 7) { - ROS_ERROR( - "JointImpedanceExampleController: Invalid or no k_gain parameters provided, aborting " - "controller init!"); - return false; - } - - if (!node_handle.getParam("d_gains", d_gains_) || d_gains_.size() != 7) { - ROS_ERROR( - "JointImpedanceExampleController: Invalid or no d_gain parameters provided, aborting " - "controller init!"); - return false; - } - double publish_rate(30.0); if (!node_handle.getParam("publish_rate", publish_rate)) { ROS_INFO_STREAM("JointImpedanceExampleController: publish_rate not found. Defaulting to " @@ -128,6 +117,11 @@ bool HIROJointImpedanceExampleController::init(hardware_interface::RobotHW* robo return false; } } + + for (int i = 0; i < 7; i++){ + k_gains_[i] = non_zero_imp_k[i]; + d_gains_[i] = non_zero_imp_d[i]; + } torques_publisher_.init(node_handle, "torque_comparison", 1); std::fill(dq_filtered_.begin(), dq_filtered_.end(), 0); @@ -136,7 +130,7 @@ bool HIROJointImpedanceExampleController::init(hardware_interface::RobotHW* robo } void HIROJointImpedanceExampleController::starting(const ros::Time& /*time*/) { - initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d; + pose_from_cb_ = cartesian_pose_handle_->getRobotState().O_T_EE_d; } void HIROJointImpedanceExampleController::xboxCommandCb(const sensor_msgs::JointState::ConstPtr& joint_pos_commands) { @@ -144,6 +138,36 @@ void HIROJointImpedanceExampleController::xboxCommandCb(const sensor_msgs::Joint this->callback_done_once = true; } +void HIROJointImpedanceExampleController::impedanceChangeBoolCb(const std_msgs::Bool::ConstPtr& impedance_change_bool){ + std::lock_guard q_and_qdot_lock_mutex(joint_position_and_velocity_d_target_mutex_); + if(impedance_change_bool->data){ + for (int i = 0; i < 7; i++){ + k_gains_[i] = 0.0; + d_gains_[i] = 0.0; + } + // pose_from_cb_ = cartesian_pose_handle_->getRobotState().O_T_EE_d; + // std::array pose_desired = pose_from_cb_; + // // std::cout << "Curr pose, impedance zero: " << pose_desired[12] << " " << pose_desired[13] << " " << pose_desired[14] << std::endl; + // cartesian_pose_handle_->setCommand(pose_desired); + // this->callback_done_once = true; + } + else{ + // pose_from_cb_ = cartesian_pose_handle_->getRobotState().O_T_EE_d; + // std::array pose_desired = pose_from_cb_; + // cartesian_pose_handle_->setCommand(pose_desired); + // for (size_t i = 0; i < 7; ++i) { + // joint_positions_[i] = cartesian_pose_handle_->getRobotState().q[i]; + // } + + for (int i = 0; i < 7; i++){ + k_gains_[i] = non_zero_imp_k[i]; + d_gains_[i] = non_zero_imp_d[i]; + } + + // std::cout << "Curr pose, impedance a trillion: " << pose_desired[12] << " " << pose_desired[13] << " " << pose_desired[14] << std::endl; + } +} + void HIROJointImpedanceExampleController::cuRoboCommandCb(const std_msgs::Float32MultiArray::ConstPtr& joint_pos_commands) { std::lock_guard q_and_qdot_lock_mutex(joint_position_and_velocity_d_target_mutex_); for (int i = 0; i < 7; i++) joint_positions_[i] = joint_pos_commands->data[i]; @@ -162,15 +186,10 @@ void HIROJointImpedanceExampleController::update(const ros::Time& /*time*/, angle_ -= 2 * M_PI; } - double delta_y = radius_ * (1 - std::cos(angle_)); - double delta_z = radius_ * std::sin(angle_); + std::array pose_desired = pose_from_cb_; + franka::RobotState robot_state = cartesian_pose_handle_->getRobotState(); - std::array pose_desired = initial_pose_; - // pose_desired[13] += delta_y; - // pose_desired[14] += delta_z; cartesian_pose_handle_->setCommand(pose_desired); - - franka::RobotState robot_state = cartesian_pose_handle_->getRobotState(); std::array coriolis = model_handle_->getCoriolis(); std::array gravity = model_handle_->getGravity(); @@ -181,12 +200,11 @@ void HIROJointImpedanceExampleController::update(const ros::Time& /*time*/, std::array tau_d_calculated; if(this->callback_done_once){ - // std::cout<< "in CALLLBACK!!" << std::endl; std::lock_guard q_and_qdot_lock_mutex(joint_position_and_velocity_d_target_mutex_); for (size_t i = 0; i < 7; ++i) { tau_d_calculated[i] = coriolis_factor_ * coriolis[i] + k_gains_[i] * (joint_positions_[i] - robot_state.q[i]) + - d_gains_[i] * (0.5 *(joint_positions_[i] - robot_state.q[i]) - dq_filtered_[i]); + d_gains_[i] * ((joint_positions_[i] - robot_state.q[i]) - dq_filtered_[i]); } joint_position_and_velocity_d_target_mutex_.unlock(); }else{ @@ -204,7 +222,6 @@ void HIROJointImpedanceExampleController::update(const ros::Time& /*time*/, for (size_t i = 0; i < 7; ++i) { joint_handles_[i].setCommand(tau_d_saturated[i]); } - if (rate_trigger_() && torques_publisher_.trylock()) { std::array tau_j = robot_state.tau_J; std::array tau_error; @@ -221,7 +238,6 @@ void HIROJointImpedanceExampleController::update(const ros::Time& /*time*/, } torques_publisher_.unlockAndPublish(); } - for (size_t i = 0; i < 7; ++i) { last_tau_d_[i] = tau_d_saturated[i] + gravity[i]; }