From 73e5a80694ee62820dae0ce097d66fcf3524b979 Mon Sep 17 00:00:00 2001 From: franzesegiovanni Date: Mon, 18 Jul 2022 18:48:43 +0200 Subject: [PATCH 1/4] moving elbow with spacenav working --- .../cartesian_variable_impedance_controller.h | 8 +- python/LfD/.vscode/c_cpp_properties.json | 27 +++++ python/LfD/.vscode/settings.json | 10 +- python/LfD/teleoperation_elbow.py | 107 ++++++++++++++++++ src/.vscode/c_cpp_properties.json | 27 +++++ src/.vscode/settings.json | 6 + ...artesian_variable_impedance_controller.cpp | 65 ++++++++--- 7 files changed, 231 insertions(+), 19 deletions(-) create mode 100644 python/LfD/.vscode/c_cpp_properties.json create mode 100644 python/LfD/teleoperation_elbow.py create mode 100644 src/.vscode/c_cpp_properties.json create mode 100644 src/.vscode/settings.json diff --git a/include/franka_human_friendly_controllers/cartesian_variable_impedance_controller.h b/include/franka_human_friendly_controllers/cartesian_variable_impedance_controller.h index 6423626..3ed410f 100644 --- a/include/franka_human_friendly_controllers/cartesian_variable_impedance_controller.h +++ b/include/franka_human_friendly_controllers/cartesian_variable_impedance_controller.h @@ -67,6 +67,8 @@ class CartesianVariableImpedanceController : public controller_interface::MultiI Eigen::Vector3d position_d_; Eigen::Quaterniond orientation_d_; + Eigen::Vector3d position_elbow_d_; + double count_vibration{10000.0}; double duration_vibration; bool vibrate= false; @@ -81,10 +83,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 +97,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_; diff --git a/python/LfD/.vscode/c_cpp_properties.json b/python/LfD/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..c79d552 --- /dev/null +++ b/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 +} \ No newline at end of file diff --git a/python/LfD/.vscode/settings.json b/python/LfD/.vscode/settings.json index 5d7d730..b9d74f0 100644 --- a/python/LfD/.vscode/settings.json +++ b/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" + ] } \ No newline at end of file diff --git a/python/LfD/teleoperation_elbow.py b/python/LfD/teleoperation_elbow.py new file mode 100644 index 0000000..5cf9d65 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/src/.vscode/c_cpp_properties.json b/src/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..c79d552 --- /dev/null +++ b/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 +} \ No newline at end of file diff --git a/src/.vscode/settings.json b/src/.vscode/settings.json new file mode 100644 index 0000000..0577eab --- /dev/null +++ b/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" + ] +} \ No newline at end of file diff --git a/src/cartesian_variable_impedance_controller.cpp b/src/cartesian_variable_impedance_controller.cpp index 4bb877b..7222441 100644 --- a/src/cartesian_variable_impedance_controller.cpp +++ b/src/cartesian_variable_impedance_controller.cpp @@ -20,10 +20,15 @@ bool CartesianVariableImpedanceController::init(hardware_interface::RobotHW* rob sub_equilibrium_pose_ = node_handle.subscribe( "/equilibrium_pose", 20, &CartesianVariableImpedanceController::equilibriumPoseCallback, this, ros::TransportHints().reliable().tcpNoDelay()); - sub_equilibrium_config_ = node_handle.subscribe( - "/equilibrium_configuration", 20, &CartesianVariableImpedanceController::equilibriumConfigurationCallback, this, - ros::TransportHints().reliable().tcpNoDelay()); + // sub_equilibrium_config_ = node_handle.subscribe( + // "/equilibrium_configuration", 20, &CartesianVariableImpedanceController::equilibriumConfigurationCallback, this, + // ros::TransportHints().reliable().tcpNoDelay()); // We want to add the subscriber to the note for reading the desired stiffness in the different directions + + sub_equilibrium_pose_elbow_ = node_handle.subscribe( + "/equilibrium_pose_elbow", 20, &CartesianVariableImpedanceController::equilibriumPoseElbowCallback, this, + ros::TransportHints().reliable().tcpNoDelay()); + sub_stiffness_ = node_handle.subscribe( "/stiffness", 20, &CartesianVariableImpedanceController::equilibriumStiffnessCallback, this, ros::TransportHints().reliable().tcpNoDelay()); @@ -36,6 +41,8 @@ bool CartesianVariableImpedanceController::init(hardware_interface::RobotHW* rob pub_cartesian_pose_= node_handle.advertise("/cartesian_pose",1); + pub_cartesian_pose_elbow= node_handle.advertise("/cartesian_pose_elbow",1); + pub_force_torque_= node_handle.advertise("/force_torque_ext",1); std::string arm_id; @@ -130,18 +137,27 @@ void CartesianVariableImpedanceController::starting(const ros::Time& /*time*/) { // get jacobian std::array jacobian_array = model_handle_->getZeroJacobian(franka::Frame::kEndEffector); + + std::array pose = model_handle_->getPose(franka::Frame::kJoint4); + + std::array jacobian_elbow_array = + model_handle_->getZeroJacobian(franka::Frame::kJoint4); // convert to eigen Eigen::Map > jacobian(jacobian_array.data()); + Eigen::Map > jacobian_elbow(jacobian_elbow_array.data()); Eigen::Map > dq_initial(initial_state.dq.data()); Eigen::Map > q_initial(initial_state.q.data()); Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data())); + Eigen::Affine3d initial_transform_elbow(Eigen::Matrix4d::Map(pose.data())); // set equilibrium point to current state position_d_ = initial_transform.translation(); // this allows the robot to start on the starting configuration orientation_d_ = Eigen::Quaterniond(initial_transform.linear()); // this allows the robot to start on the starting configuration + + position_elbow_d_= initial_transform_elbow.translation(); //position_d_target_ = initial_transform.translation(); //orientation_d_target_ = Eigen::Quaterniond(initial_transform.linear()); // set nullspace equilibrium configuration to initial q - q_d_nullspace_ = q_initial; + //q_d_nullspace_ = q_initial; force_torque_old.setZero(); double time_old=ros::Time::now().toSec(); count_vibration=1000; @@ -156,10 +172,12 @@ void CartesianVariableImpedanceController::update(const ros::Time& /*time*/, Eigen::Map > mass(mass_array.data()); std::array jacobian_array = model_handle_->getZeroJacobian(franka::Frame::kEndEffector); - + std::array jacobian_elbow_array = + model_handle_->getZeroJacobian(franka::Frame::kJoint4); // convert to Eigen Eigen::Map > coriolis(coriolis_array.data()); Eigen::Map > jacobian(jacobian_array.data()); + Eigen::Map > jacobian_elbow(jacobian_elbow_array.data()); Eigen::Map > q(robot_state.q.data()); Eigen::Map > dq(robot_state.dq.data()); double time_=ros::Time::now().toSec(); @@ -171,6 +189,12 @@ void CartesianVariableImpedanceController::update(const ros::Time& /*time*/, Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); Eigen::Vector3d position(transform.translation()); Eigen::Quaterniond orientation(transform.linear()); + + std::array pose = model_handle_->getPose(franka::Frame::kJoint4); + Eigen::Affine3d transform_elbow(Eigen::Matrix4d::Map(pose.data())); + Eigen::Vector3d position_elbow(transform_elbow.translation()); + Eigen::Quaterniond orientation_elbow(transform_elbow.linear()); + Eigen::Matrix tau_f; Eigen::MatrixXd jacobian_transpose_pinv; Eigen::MatrixXd Null_mat; @@ -228,11 +252,24 @@ void CartesianVariableImpedanceController::update(const ros::Time& /*time*/, pose_msg.pose.orientation.z=orientation.z(); pose_msg.pose.orientation.w=orientation.w(); pub_cartesian_pose_.publish(pose_msg); + + geometry_msgs::PoseStamped elbow_msg; + elbow_msg.pose.position.x=position_elbow[0]; + elbow_msg.pose.position.y=position_elbow[1]; + elbow_msg.pose.position.z=position_elbow[2]; + elbow_msg.pose.orientation.x=orientation_elbow.x(); + elbow_msg.pose.orientation.y=orientation_elbow.y(); + elbow_msg.pose.orientation.z=orientation_elbow.z(); + elbow_msg.pose.orientation.w=orientation_elbow.w(); + pub_cartesian_pose_elbow.publish(elbow_msg); // compute error to desired pose // position error Eigen::Matrix error; error.head(3) << position - position_d_; + Eigen::Matrix error_elbow; + error_elbow.head(3) << position_elbow - position_elbow_d_; + // orientation error if (orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0) { orientation.coeffs() << -orientation.coeffs(); @@ -266,8 +303,8 @@ void CartesianVariableImpedanceController::update(const ros::Time& /*time*/, // nullspace PD control with damping ratio = 1 tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) - jacobian.transpose() * jacobian_transpose_pinv) * - (nullspace_stiffness_ * null_vect - - 1*(2.0 * sqrt(nullspace_stiffness_)) * dq); //double critic damping + (jacobian_elbow.transpose() * + (-cartesian_stiffness_ * error_elbow - cartesian_damping_ * (jacobian_elbow * dq)) ); //double critic damping tau_joint_limit.setZero(); if (q(0)>2.85) { tau_joint_limit(0)=-10; } if (q(0)<-2.85) { tau_joint_limit(0)=+10; } @@ -429,15 +466,13 @@ void CartesianVariableImpedanceController::equilibriumPoseCallback( orientation_d_.coeffs() << -orientation_d_.coeffs(); } } -void CartesianVariableImpedanceController::equilibriumConfigurationCallback( const std_msgs::Float32MultiArray::ConstPtr& joint) { - int i = 0; - for(std::vector::const_iterator it = joint->data.begin(); it != joint->data.end(); ++it) - { - q_d_nullspace_[i] = *it; - i++; - } - return; + + +void CartesianVariableImpedanceController::equilibriumPoseElbowCallback( + const geometry_msgs::PoseStampedConstPtr& msg) { + position_elbow_d_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; } + void CartesianVariableImpedanceController::equilibriumVibrationCallback( const std_msgs::Float32::ConstPtr& vibration_msg) { count_vibration = 0; duration_vibration = vibration_msg->data; From f080262389260cedfa7c123eacbb9050244a6e4e Mon Sep 17 00:00:00 2001 From: franzesegiovanni Date: Thu, 28 Jul 2022 13:27:32 +0200 Subject: [PATCH 2/4] fixed small bug on elbow control --- src/cartesian_variable_impedance_controller.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/cartesian_variable_impedance_controller.cpp b/src/cartesian_variable_impedance_controller.cpp index 7222441..3bf8ad0 100644 --- a/src/cartesian_variable_impedance_controller.cpp +++ b/src/cartesian_variable_impedance_controller.cpp @@ -152,7 +152,7 @@ void CartesianVariableImpedanceController::starting(const ros::Time& /*time*/) { // set equilibrium point to current state position_d_ = initial_transform.translation(); // this allows the robot to start on the starting configuration orientation_d_ = Eigen::Quaterniond(initial_transform.linear()); // this allows the robot to start on the starting configuration - + position_elbow_d_.setZero(); position_elbow_d_= initial_transform_elbow.translation(); //position_d_target_ = initial_transform.translation(); //orientation_d_target_ = Eigen::Quaterniond(initial_transform.linear()); @@ -268,8 +268,13 @@ void CartesianVariableImpedanceController::update(const ros::Time& /*time*/, error.head(3) << position - position_d_; Eigen::Matrix error_elbow; + error_elbow.setZero(); error_elbow.head(3) << position_elbow - position_elbow_d_; + + error_elbow[0]=std::max(-0.02, std::min(error_elbow[0], 0.02)); + error_elbow[1]=std::max(-0.02, std::min(error_elbow[1], 0.02)); + error_elbow[2]=std::max(-0.02, std::min(error_elbow[2], 0.02)); // orientation error if (orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0) { orientation.coeffs() << -orientation.coeffs(); @@ -298,6 +303,10 @@ void CartesianVariableImpedanceController::update(const ros::Time& /*time*/, null_vect(5)=(q_d_nullspace_(5) - q(5)); null_vect(6)=(q_d_nullspace_(6) - q(6)); // Cartesian PD control with damping ratio = 1 + error[0]=std::max(-0.02, std::min(error[0], 0.02)); + error[1]=std::max(-0.02, std::min(error[1], 0.02)); + error[2]=std::max(-0.02, std::min(error[2], 0.02)); + tau_task << jacobian.transpose() * (-cartesian_stiffness_ * error - cartesian_damping_ * (jacobian * dq)); //double critic damping // nullspace PD control with damping ratio = 1 From 45d4356844c70077d7546b30d2f434a71b33a858 Mon Sep 17 00:00:00 2001 From: franzesegiovanni Date: Fri, 29 Jul 2022 17:49:02 +0200 Subject: [PATCH 3/4] added the elbow orientation --- .../cartesian_variable_impedance_controller.h | 1 + ...artesian_variable_impedance_controller.cpp | 32 +++++++++++++++---- 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/include/franka_human_friendly_controllers/cartesian_variable_impedance_controller.h b/include/franka_human_friendly_controllers/cartesian_variable_impedance_controller.h index 3ed410f..76cb527 100644 --- a/include/franka_human_friendly_controllers/cartesian_variable_impedance_controller.h +++ b/include/franka_human_friendly_controllers/cartesian_variable_impedance_controller.h @@ -66,6 +66,7 @@ class CartesianVariableImpedanceController : public controller_interface::MultiI Eigen::Matrix stiff_; Eigen::Vector3d position_d_; Eigen::Quaterniond orientation_d_; + Eigen::Quaterniond orientation_elbow_d_; Eigen::Vector3d position_elbow_d_; diff --git a/src/cartesian_variable_impedance_controller.cpp b/src/cartesian_variable_impedance_controller.cpp index 3bf8ad0..817ea4b 100644 --- a/src/cartesian_variable_impedance_controller.cpp +++ b/src/cartesian_variable_impedance_controller.cpp @@ -154,6 +154,7 @@ void CartesianVariableImpedanceController::starting(const ros::Time& /*time*/) { orientation_d_ = Eigen::Quaterniond(initial_transform.linear()); // this allows the robot to start on the starting configuration position_elbow_d_.setZero(); position_elbow_d_= initial_transform_elbow.translation(); + orientation_elbow_d_= Eigen::Quaterniond(initial_transform_elbow.linear()); //position_d_target_ = initial_transform.translation(); //orientation_d_target_ = Eigen::Quaterniond(initial_transform.linear()); // set nullspace equilibrium configuration to initial q @@ -271,10 +272,23 @@ void CartesianVariableImpedanceController::update(const ros::Time& /*time*/, error_elbow.setZero(); error_elbow.head(3) << position_elbow - position_elbow_d_; + if (orientation_elbow_d_.coeffs().dot(orientation_elbow.coeffs()) < 0.0) { + orientation_elbow.coeffs() << -orientation_elbow.coeffs(); + } + // "difference" quaternion + Eigen::Quaterniond error_quaternion_elbow(orientation_elbow * orientation_elbow_d_.inverse()); + // convert to axis angle + Eigen::AngleAxisd error_quaternion_angle_axis_elbow(error_quaternion_elbow); + // compute "orientation error" + error_elbow.tail(3) << error_quaternion_angle_axis_elbow.axis() * error_quaternion_angle_axis_elbow.angle(); + + error_elbow[0]=std::max(-0.04, std::min(error_elbow[0], 0.04)); + error_elbow[1]=std::max(-0.04, std::min(error_elbow[1], 0.04)); + error_elbow[2]=std::max(-0.04, std::min(error_elbow[2], 0.04)); + error_elbow[3]=std::max(-0.04, std::min(error_elbow[3], 0.04)); + error_elbow[4]=std::max(-0.04, std::min(error_elbow[4], 0.04)); + error_elbow[5]=std::max(-0.04, std::min(error_elbow[5], 0.04)); - error_elbow[0]=std::max(-0.02, std::min(error_elbow[0], 0.02)); - error_elbow[1]=std::max(-0.02, std::min(error_elbow[1], 0.02)); - error_elbow[2]=std::max(-0.02, std::min(error_elbow[2], 0.02)); // orientation error if (orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0) { orientation.coeffs() << -orientation.coeffs(); @@ -303,9 +317,9 @@ void CartesianVariableImpedanceController::update(const ros::Time& /*time*/, null_vect(5)=(q_d_nullspace_(5) - q(5)); null_vect(6)=(q_d_nullspace_(6) - q(6)); // Cartesian PD control with damping ratio = 1 - error[0]=std::max(-0.02, std::min(error[0], 0.02)); - error[1]=std::max(-0.02, std::min(error[1], 0.02)); - error[2]=std::max(-0.02, std::min(error[2], 0.02)); + error[0]=std::max(-0.04, std::min(error[0], 0.04)); + error[1]=std::max(-0.04, std::min(error[1], 0.04)); + error[2]=std::max(-0.04, std::min(error[2], 0.04)); tau_task << jacobian.transpose() * (-cartesian_stiffness_ * error - cartesian_damping_ * (jacobian * dq)); //double critic damping @@ -480,6 +494,12 @@ void CartesianVariableImpedanceController::equilibriumPoseCallback( void CartesianVariableImpedanceController::equilibriumPoseElbowCallback( const geometry_msgs::PoseStampedConstPtr& msg) { position_elbow_d_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; + Eigen::Quaterniond last_orientation_elbow_d_(orientation_elbow_d_); + orientation_elbow_d_.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y, + msg->pose.orientation.z, msg->pose.orientation.w; + if (last_orientation_elbow_d_.coeffs().dot(orientation_elbow_d_.coeffs()) < 0.0) { + orientation_elbow_d_.coeffs() << -orientation_elbow_d_.coeffs(); + } } void CartesianVariableImpedanceController::equilibriumVibrationCallback( const std_msgs::Float32::ConstPtr& vibration_msg) { From ff2bf579d7054c4437b031d57e74fdbfc741a078 Mon Sep 17 00:00:00 2001 From: Giovanni Franzese <36767288+franzesegiovanni@users.noreply.github.com> Date: Thu, 20 Oct 2022 15:25:24 +0100 Subject: [PATCH 4/4] cleaned joint impedance code --- .../joint_variable_impedance_controller.h | 46 ++-------- src/joint_variable_impedance_controller.cpp | 90 ++++--------------- 2 files changed, 20 insertions(+), 116 deletions(-) diff --git a/include/franka_human_friendly_controllers/joint_variable_impedance_controller.h b/include/franka_human_friendly_controllers/joint_variable_impedance_controller.h index ad806b9..a34383e 100644 --- a/include/franka_human_friendly_controllers/joint_variable_impedance_controller.h +++ b/include/franka_human_friendly_controllers/joint_variable_impedance_controller.h @@ -49,10 +49,7 @@ class JointVariableImpedanceController : public controller_interface::MultiInter std::unique_ptr model_handle_; std::vector 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,26 +58,8 @@ class JointVariableImpedanceController : public controller_interface::MultiInter Eigen::Matrix q_d_; Eigen::Matrix joint_stiffness_target_; Eigen::Matrix joint_damping_target_; - Eigen::Matrix force_torque; - Eigen::Matrix force_torque_old; - Eigen::Matrix stiff_; - std::array q_start_ik; - Eigen::Vector3d position_d_; - Eigen::Quaterniond orientation_d_; - std::array goal;//{0.0,0.0,0.0,0.0,0.0,0.0,0.0}; - Eigen::Matrix goal_; - std::array mass_goal_; - std::array 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 F_x_Ctotal_ = {{-0.01, 0.0, 0.03}}; // dummie parameter to get goal mass matrix - - Eigen::Matrix K_; - Eigen::Matrix D_; - - // Dynamic reconfigure + + // Dynamic reconfigure of stiffness std::unique_ptr> dynamic_server_compliance_joint_param_; @@ -88,36 +67,21 @@ class JointVariableImpedanceController : public controller_interface::MultiInter 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 force_torque; + Eigen::Matrix 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 _position_joint_handles; - void calculateDamping(Eigen::Matrix& goal); - - }; diff --git a/src/joint_variable_impedance_controller.cpp b/src/joint_variable_impedance_controller.cpp index dcf6930..d0ff991 100644 --- a/src/joint_variable_impedance_controller.cpp +++ b/src/joint_variable_impedance_controller.cpp @@ -16,16 +16,7 @@ namespace franka_human_friendly_controllers { bool JointVariableImpedanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) { - std::vector cartesian_stiffness_vector; - std::vector cartesian_damping_vector; - - sub_stiffness_ = node_handle.subscribe( - "/stiffness", 20, &JointVariableImpedanceController::equilibriumStiffnessCallback, this, - ros::TransportHints().reliable().tcpNoDelay()); - // sub_equilibrium_pose_ik = node_handle.subscribe( - // "/equilibrium_pose", 1, &JointVariableImpedanceController::equilibriumConfigurationIKCallback, this, - // ros::TransportHints().reliable().tcpNoDelay()); sub_equilibrium_config_ = node_handle.subscribe( "/equilibrium_configuration", 20, &JointVariableImpedanceController::equilibriumConfigurationCallback, this, ros::TransportHints().reliable().tcpNoDelay()); @@ -33,9 +24,10 @@ bool JointVariableImpedanceController::init(hardware_interface::RobotHW* robot_h pub_stiff_update_ = node_handle.advertise( "/dynamic_reconfigure_compliance_param_node/parameter_updates", 5); - + // PUBLISHERS TO READ POSITION AND FORCE AT CONTROL FREQUENCY pub_cartesian_pose_= node_handle.advertise("/cartesian_pose",1); pub_force_torque_= node_handle.advertise("/force_torque_ext",1); + // END std::string arm_id; if (!node_handle.getParam("arm_id", arm_id)) { @@ -111,11 +103,6 @@ bool JointVariableImpedanceController::init(hardware_interface::RobotHW* robot_h dynamic_server_compliance_joint_param_->setCallback( boost::bind(&JointVariableImpedanceController::complianceJointParamCallback, this, _1, _2)); - position_d_.setZero(); - orientation_d_.coeffs() << 0.0, 0.0, 0.0, 1.0; - - stiff_.setZero(); - return true; } @@ -123,13 +110,10 @@ void JointVariableImpedanceController::starting(const ros::Time& /*time*/) { // compute initial velocity with jacobian and set x_attractor and q_d_nullspace // to initial configuration franka::RobotState initial_state = state_handle_->getRobotState(); - // get jacobian Eigen::Map > dq_initial(initial_state.dq.data()); Eigen::Map > q_initial(initial_state.q.data()); - Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data())); - q_d_ = q_initial; + q_d_ = q_initial; // this command sets the goal of the robot to the starting position. It allows to not have strange behavious of the controller force_torque_old.setZero(); - double time_old=ros::Time::now().toSec(); } void JointVariableImpedanceController::update(const ros::Time& /*time*/, @@ -137,8 +121,6 @@ void JointVariableImpedanceController::update(const ros::Time& /*time*/, // get state variables franka::RobotState robot_state = state_handle_->getRobotState(); std::array coriolis_array = model_handle_->getCoriolis(); - std::array mass_array = model_handle_->getMass(); - Eigen::Map > mass(mass_array.data()); std::array jacobian_array = model_handle_->getZeroJacobian(franka::Frame::kEndEffector); @@ -147,18 +129,17 @@ void JointVariableImpedanceController::update(const ros::Time& /*time*/, Eigen::Map > jacobian(jacobian_array.data()); Eigen::Map > q(robot_state.q.data()); Eigen::Map > dq(robot_state.dq.data()); - double time_=ros::Time::now().toSec(); - Eigen::Map > tau_J_d( // NOLINT (readability-identifier-naming) + Eigen::Map > tau_J_d( robot_state.tau_J_d.data()); + + // STREAM OF DATA ON POSITION AND FORCE ON THE END EFFECTOR AT THE SAME FREQUENCY OF THE CONTROLLER Eigen::Map > tau_ext(robot_state.tau_ext_hat_filtered.data()); - std::array gravity = model_handle_->getGravity(); Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); Eigen::Vector3d position(transform.translation()); Eigen::Quaterniond orientation(transform.linear()); Eigen::Matrix tau_f; Eigen::MatrixXd jacobian_transpose_pinv; - Eigen::MatrixXd Null_mat; pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv); tau_f(0) = FI_11/(1+exp(-FI_21*(dq(0)+FI_31))) - TAU_F_CONST_1; @@ -168,9 +149,7 @@ void JointVariableImpedanceController::update(const ros::Time& /*time*/, tau_f(4) = FI_15/(1+exp(-FI_25*(dq(4)+FI_35))) - TAU_F_CONST_5; tau_f(5) = FI_16/(1+exp(-FI_26*(dq(5)+FI_36))) - TAU_F_CONST_6; tau_f(6) = FI_17/(1+exp(-FI_27*(dq(6)+FI_37))) - TAU_F_CONST_7; - force_torque=force_torque-jacobian_transpose_pinv*(tau_ext-tau_f); - // publish force, torque filter_step=filter_step+1; filter_step_=10; @@ -197,20 +176,12 @@ void JointVariableImpedanceController::update(const ros::Time& /*time*/, pose_msg.pose.orientation.z=orientation.z(); pose_msg.pose.orientation.w=orientation.w(); pub_cartesian_pose_.publish(pose_msg); - pub_cartesian_pose_.publish(pose_msg); - // compute error to desired pose - Eigen::VectorXd tau_joint(7), tau_d(7), error_vect(7), tau_joint_limit(7); +// END OF STREAM OF DATA. THIS PUBBLISHING CAN MAKE THE CONTROLLER SLOWER. + Eigen::VectorXd tau_joint(7), tau_d(7), error_vect(7), tau_joint_limit(7); error_vect.setZero(); - // error_vect(0)=std::max(-0.1,std::min((q_d_(0) - q(0)),0.1)); - // error_vect(1)=std::max(-0.1,std::min((q_d_(1) - q(1)),0.1)); - // error_vect(2)=std::max(-0.1,std::min((q_d_(2) - q(2)),0.1)); - // error_vect(3)=std::max(-0.1,std::min((q_d_(3) - q(3)),0.1)); - // error_vect(4)=std::max(-0.1,std::min((q_d_(4) - q(4)),0.1)); - // error_vect(5)=std::max(-0.1,std::min((q_d_(5) - q(5)),0.1)); - // error_vect(6)=std::max(-0.1,std::min((q_d_(6) - q(6)),0.1)); error_vect(0)=(q_d_(0) - q(0)); error_vect(1)=(q_d_(1) - q(1)); error_vect(2)=(q_d_(2) - q(2)); @@ -218,7 +189,10 @@ void JointVariableImpedanceController::update(const ros::Time& /*time*/, error_vect(4)=(q_d_(4) - q(4)); error_vect(5)=(q_d_(5) - q(5)); error_vect(6)=(q_d_(6) - q(6)); - tau_joint << joint_stiffness_target_ * (error_vect) - joint_damping_target_ * (dq); //double critic damping + + tau_joint << joint_stiffness_target_ * (error_vect) - joint_damping_target_ * (dq); + + // calculation of a repulsor when going close to a joint limit tau_joint_limit.setZero(); if (q(0)>2.85) { tau_joint_limit(0)=-10; } if (q(0)<-2.85) { tau_joint_limit(0)=+10; } @@ -235,7 +209,7 @@ void JointVariableImpedanceController::update(const ros::Time& /*time*/, if (q(6)>2.8) { tau_joint_limit(6)=-10; } if (q(6)<-2.8) { tau_joint_limit(6)=+10; } // Desired torque - tau_d << tau_joint + tau_joint_limit; + tau_d << tau_joint + coriolis + tau_joint_limit; // Saturate torque rate to avoid discontinuities tau_d << saturateTorqueRate(tau_d, tau_J_d); for (size_t i = 0; i < 7; ++i) { @@ -255,27 +229,6 @@ Eigen::Matrix JointVariableImpedanceController::saturateTorqueRate return tau_d_saturated; } -void JointVariableImpedanceController::equilibriumStiffnessCallback( - const std_msgs::Float32MultiArray::ConstPtr& stiffness_){ - - int i = 0; - // print all the remaining numbers - for(std::vector::const_iterator it = stiffness_->data.begin(); it != stiffness_->data.end(); ++it) - { - stiff_[i] = *it; - i++; - } - for (int i = 0; i < 7; i++){ - for (int j = 0; j < 7; j++) { - joint_stiffness_target_(i,j)=std::max(std::min(stiff_[i+j], float(100.0)), float(0.0)); - } - } - - ROS_INFO_STREAM("Stiffness matrix is:" << joint_stiffness_target_); - calculateDamping(q_d_); //check what damping ratio is actually taking - ROS_INFO_STREAM("Damping matrix is:" << joint_damping_target_); -} - void JointVariableImpedanceController::complianceJointParamCallback( franka_human_friendly_controllers::compliance_joint_paramConfig& config, uint32_t /*level*/) { joint_stiffness_target_.setIdentity(); @@ -293,25 +246,12 @@ void JointVariableImpedanceController::complianceJointParamCallback( joint_damping_target_(3,3)=2*damping_ratio*joint_stiffness_target_(3,3) ; joint_damping_target_(4,4)=2*damping_ratio*joint_stiffness_target_(4,4) ; joint_damping_target_(5,5)=2*damping_ratio*joint_stiffness_target_(5,5) ; - joint_damping_target_(5,5)=2*damping_ratio*joint_stiffness_target_(6,6) ; - ROS_INFO_STREAM("Stiffness matrix is:" << joint_stiffness_target_); - ROS_INFO_STREAM("Damping matrix is:" << joint_damping_target_); + joint_damping_target_(6,6)=2*damping_ratio*joint_stiffness_target_(6,6) ; } void JointVariableImpedanceController::equilibriumConfigurationCallback( const sensor_msgs::JointState::ConstPtr& joint) { - int i = 0; - Eigen::Matrix q_d_damp; - - for(int i=0; i<7; ++i) - { - q_d_damp[i] = joint->position[i]; - } - - calculateDamping(q_d_damp); - ROS_INFO_STREAM("Stiffness matrix is:" << joint_stiffness_target_); - ROS_INFO_STREAM("Damping matrix is:" << joint_damping_target_); - + for(int i=0; i<7; ++i) { q_d_[i] = joint->position[i];