Joint impedance control cleaning on different branch

include/franka_human_friendly_controllers/cartesian_variable_impedance_controller.h

@@ -66,6 +66,9 @@ class CartesianVariableImpedanceController : public controller_interface::MultiI
   Eigen::Matrix<float, 7, 1> stiff_;
   Eigen::Vector3d position_d_;
   Eigen::Quaterniond orientation_d_;
+  Eigen::Quaterniond orientation_elbow_d_;
+
+  Eigen::Vector3d position_elbow_d_;
 
   double count_vibration{10000.0};
   double duration_vibration;
@@ -81,10 +84,9 @@ class CartesianVariableImpedanceController : public controller_interface::MultiI
   ros::Subscriber sub_equilibrium_pose_;
   void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg);
 
-  // Configuration pose subscriber
-  ros::Subscriber sub_equilibrium_config_;
-  void equilibriumConfigurationCallback( const std_msgs::Float32MultiArray::ConstPtr& joint);
 
+  ros::Subscriber sub_equilibrium_pose_elbow_;
+  void equilibriumPoseElbowCallback(const geometry_msgs::PoseStampedConstPtr& msg) ;
 
   // Multi directional stiffness stiffnes
   ros::Subscriber sub_stiffness_;
@@ -96,6 +98,7 @@ class CartesianVariableImpedanceController : public controller_interface::MultiI
   ros::Publisher pub_stiff_update_;
 
   ros::Publisher pub_cartesian_pose_;
+  ros::Publisher pub_cartesian_pose_elbow;
   ros::Publisher pub_force_torque_;
 
 

include/franka_human_friendly_controllers/joint_variable_impedance_controller.h

@@ -49,10 +49,7 @@ class JointVariableImpedanceController : public controller_interface::MultiInter
   std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
   std::vector<hardware_interface::JointHandle> joint_handles_;
 
-  double nullspace_stiffness_{0.0};
-  double nullspace_stiffness_target_{0.0};
   double dt{0.001};
-  double time_old;
   int alpha;
   int filter_step{0};
   int filter_step_;
@@ -61,63 +58,30 @@ class JointVariableImpedanceController : public controller_interface::MultiInter
   Eigen::Matrix<double, 7, 1> q_d_;
   Eigen::Matrix<double, 7, 7> joint_stiffness_target_;
   Eigen::Matrix<double, 7, 7> joint_damping_target_;
-  Eigen::Matrix<double, 6, 1> force_torque;
-  Eigen::Matrix<double, 6, 1> force_torque_old;
-  Eigen::Matrix<float, 7, 1> stiff_;
-  std::array<double, 7> q_start_ik;
-  Eigen::Vector3d position_d_;
-  Eigen::Quaterniond orientation_d_;
-  std::array<double,7> goal;//{0.0,0.0,0.0,0.0,0.0,0.0,0.0};  
-  Eigen::Matrix<double, 7, 1> goal_;
-  std::array<double, 49> mass_goal_;
-  std::array<double, 9> total_inertia_ = {{0.001, 0.0, 0.0, 0.0, 0.0025, 0.0, 0.0, 0.0, 0.0017}}; 
-  //total_inertia_ << 0.001, 0.0, 0.0, 0.0, 0.0025, 0.0, 0.0, 0.0, 0.0017; // dummie parameter to get goal mass matrix
-
-  double total_mass_ = {0.73}; // dummie parameter to get goal mass matrix
-  //double total_mass_ = {0.9}; // dummie parameter to get goal mass matrix
-  std::array<double, 3> F_x_Ctotal_ = {{-0.01, 0.0, 0.03}}; // dummie parameter to get goal mass matrix
-
-  Eigen::Matrix<double, 7, 7> K_;
-  Eigen::Matrix<double, 7, 7> D_;
-
-  // Dynamic reconfigure
+
+  // Dynamic reconfigure of stiffness 
   std::unique_ptr<dynamic_reconfigure::Server<franka_human_friendly_controllers::compliance_joint_paramConfig>>
       dynamic_server_compliance_joint_param_;
 
   ros::NodeHandle dynamic_reconfigure_compliance_joint_param_node_;
   void complianceJointParamCallback(franka_human_friendly_controllers::compliance_joint_paramConfig& config,
                                uint32_t level);
 
-  // Equilibrium pose subscriber
-  ros::Subscriber sub_equilibrium_pose_;
-  void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg);
-
-  // Configuration pose subscriber
+  // Configuration joint configuration subscriber
   ros::Subscriber sub_equilibrium_config_;
   void equilibriumConfigurationCallback( const sensor_msgs::JointState::ConstPtr& joint);
 
 
-  // Equilibrium pose subscriber IK
-  ros::Subscriber sub_equilibrium_pose_ik;
-  //void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg);
-//   void equilibriumConfigurationIKCallback( const geometry_msgs::PoseStampedConstPtr& msg);
-  // Multi directional stiffness stiffnes
-  ros::Subscriber sub_stiffness_;
-  void equilibriumStiffnessCallback(const std_msgs::Float32MultiArray::ConstPtr& stiffness_);
+  Eigen::Matrix<double, 6, 1> force_torque;
+  Eigen::Matrix<double, 6, 1> force_torque_old;
 
   ros::Publisher pub_stiff_update_;
   ros::Publisher pub_cartesian_pose_;
   ros::Publisher pub_force_torque_;
 
-//   PandaTracIK _panda_ik_service;
-//   KDL::JntArray _joints_result;
-
   hardware_interface::PositionJointInterface *_position_joint_interface;
   std::vector<hardware_interface::JointHandle> _position_joint_handles;
 
-  void calculateDamping(Eigen::Matrix<double, 7, 1>& goal);
-
-
 };
 
 

python/LfD/.vscode/c_cpp_properties.json

@@ -0,0 +1,27 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/pandarobot/Documents/Francesco_ws/devel/include/**",
+        "/opt/ros/melodic/include/**",
+        "/home/pandarobot/Documents/Francesco_ws/src/franka_ros/franka_control/include/**",
+        "/home/pandarobot/Documents/Francesco_ws/src/franka_ros/franka_example_controllers/include/**",
+        "/home/pandarobot/Documents/Francesco_ws/src/franka_ros/franka_gazebo/include/**",
+        "/home/pandarobot/Documents/Francesco_ws/src/franka_ros/franka_gripper/include/**",
+        "/home/pandarobot/Documents/Francesco_ws/src/franka_ros/franka_human_friendly_controllers/include/**",
+        "/home/pandarobot/Documents/Francesco_ws/src/franka_ros/franka_hw/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

python/LfD/.vscode/settings.json

@@ -1,3 +1,11 @@
 {
-    "ros.distro": "noetic"
+    "ros.distro": "noetic",
+    "python.autoComplete.extraPaths": [
+        "/home/pandarobot/Documents/Francesco_ws/devel/lib/python2.7/dist-packages",
+        "/opt/ros/melodic/lib/python2.7/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/pandarobot/Documents/Francesco_ws/devel/lib/python2.7/dist-packages",
+        "/opt/ros/melodic/lib/python2.7/dist-packages"
+    ]
 }

python/LfD/teleoperation_elbow.py

@@ -0,0 +1,107 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on July 6 2022
+
+@author: Giovanni Franzese, Cognitive Robotics, TU Delft
+Run this node in a terminal. 
+Remember to run the spacenav launch file 
+$ roslaunch spacenav_node classic.launch 
+and to install the space nav before with 
+$ sudo apt install spacenavd
+$ sudo apt install ros-indigo-spacenav-node
+"""
+
+import rospy
+import numpy as np
+import quaternion # pip install numpy-quaternion
+from quaternion import from_euler_angles
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import PoseStamped
+from std_msgs.msg import Float32MultiArray, Float32
+from sensor_msgs.msg import Joy
+from pynput.keyboard import Listener, KeyCode
+from scipy.spatial.transform import Rotation
+
+
+class Teleoperation():
+    def __init__(self):
+        self.control_freq=rospy.Rate(100)
+        self.offset = [0, 0, 0, 0, 0, 0]
+        self.curr_pos = None
+        self.joint_pos = None
+        self.joy_sub=rospy.Subscriber("/spacenav/joy", Joy, self.spacenav_callback)
+        self.pos_sub=rospy.Subscriber("/cartesian_pose_elbow", PoseStamped, self.ee_pos_callback)
+        self.goal_pub = rospy.Publisher('/equilibrium_pose_elbow', PoseStamped, queue_size=0)  
+        #self.curr_pos = np.array([0.5, 0, 0.5])
+        #self.curr_ori = np.array([1, 0, 0, 0])
+
+
+    def spacenav_callback(self, data):
+        self.offset[0]= 0.002*data.axes[0]
+        self.offset[1]= 0.002*data.axes[1]
+        self.offset[2]= 0.002*data.axes[2]
+        self.offset[3]= 0.02*data.axes[3]
+        self.offset[4]= 0.02*data.axes[4] 
+        self.offset[5]= 0.02*data.axes[5]
+
+    def ee_pos_callback(self, data):
+        self.curr_pos = np.array([data.pose.position.x, data.pose.position.y, data.pose.position.z])
+        self.curr_ori = np.array([data.pose.orientation.w, data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z])
+
+
+
+    def spacenav(self):
+        # robot = rtb.models.DH.Panda()
+        # local_pos = curr_pos.reshape(1, 3)
+        # x_new = local_pos[0][0]
+        # y_new = local_pos[0][1]
+        # z_new = local_pos[0][2]
+        # joint=self.curr_orijoint_pos[0:7].reshape(1, -1)
+        # fw_pose = robot.fkine(joint)
+        # goal_ori = fw_pose.R
+        # rot = Rotation.from_quat(self.curr_ori)
+        # goal_ori_eul = rot.as_euler('xyz')
+        goal = PoseStamped()
+        goal.header.seq = 1
+        goal.header.stamp = rospy.Time.now()
+        goal.header.frame_id = "map"
+
+        goal.pose.position.x =  self.curr_pos[0]
+        goal.pose.position.y =  self.curr_pos[1]
+        goal.pose.position.z =  self.curr_pos[2]
+            #print("quat", quat) 
+        goal.pose.orientation.w = self.curr_ori[0]    
+        goal.pose.orientation.x = self.curr_ori[1] 
+        goal.pose.orientation.y = self.curr_ori[2] 
+        goal.pose.orientation.z = self.curr_ori[3] 
+        self.goal_pub.publish(goal)  
+        quat_goal=np.quaternion(self.curr_ori[0],self.curr_ori[1],self.curr_ori[2],self.curr_ori[3])
+
+        while not rospy.is_shutdown(): 
+
+            goal.pose.position.x = goal.pose.position.x + self.offset[0]
+            goal.pose.position.y = goal.pose.position.y + self.offset[1]
+            goal.pose.position.z = goal.pose.position.z + self.offset[2]
+            alpha_beta_gamma=np.array([self.offset[3], self.offset[4], self.offset[5]])
+            q_delta=from_euler_angles(alpha_beta_gamma)  
+            quat_goal=q_delta*quat_goal
+
+            goal.pose.orientation.w = quat_goal.w   
+            goal.pose.orientation.x = quat_goal.x
+            goal.pose.orientation.y = quat_goal.y
+            goal.pose.orientation.z = quat_goal.z
+
+            self.goal_pub.publish(goal)            
+
+            self.control_freq.sleep()
+
+
+#%%    
+if __name__ == '__main__':
+    rospy.init_node('Teleoperation', anonymous=True)
+
+#%%
+    teleoperation=Teleoperation()
+    rospy.sleep(2)
+    teleoperation.spacenav()

src/.vscode/c_cpp_properties.json

@@ -0,0 +1,27 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/pandarobot/Documents/Francesco_ws/devel/include/**",
+        "/opt/ros/melodic/include/**",
+        "/home/pandarobot/Documents/Francesco_ws/src/franka_ros/franka_control/include/**",
+        "/home/pandarobot/Documents/Francesco_ws/src/franka_ros/franka_example_controllers/include/**",
+        "/home/pandarobot/Documents/Francesco_ws/src/franka_ros/franka_gazebo/include/**",
+        "/home/pandarobot/Documents/Francesco_ws/src/franka_ros/franka_gripper/include/**",
+        "/home/pandarobot/Documents/Francesco_ws/src/franka_ros/franka_human_friendly_controllers/include/**",
+        "/home/pandarobot/Documents/Francesco_ws/src/franka_ros/franka_hw/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

src/.vscode/settings.json

@@ -0,0 +1,6 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/pandarobot/Documents/Francesco_ws/devel/lib/python2.7/dist-packages",
+        "/opt/ros/melodic/lib/python2.7/dist-packages"
+    ]
+}