modified and tested for UR5 real

control_algorithms/admittance/config/AdmittanceParams.yaml

@@ -41,8 +41,9 @@ stiffness_coupling: [10,0,0,0,0,0,
 base_link: "base_link"
 end_link: "wrist_3_link"
 # [position (x, y, z)  orientation (x, y, z, w)]
-desired_pose:  [0.0,0.40,0.50,       0.707, -0.707, 0.0, 0.0]
+# desired_pose:  [0.4,0.0,0.50,       0.707, -0.707, 0.0, 0.0]
 
+desired_pose:  [0.5,0.0,0.50,       0.477, -0.480, 0.646, -0.353]
 workspace_limits: [-0.50, 0.50, 0.25, 0.80, 0.30, 0.75]
 
 arm_max_vel: 1.5

control_algorithms/admittance/include/admittance/Admittance.h

@@ -87,6 +87,8 @@ class Admittance
   double arm_max_vel_;
   double arm_max_acc_;
 
+  double force_x_pre, force_y_pre, force_z_pre;
+
 public:
   Admittance(ros::NodeHandle &n, double frequency,
                       std::string topic_arm_state,

control_algorithms/admittance/launch/Admittance.launch

@@ -6,7 +6,7 @@
 
   <arg name="TOPIC_ARM_STATE" default="/cartesian_velocity_controller/ee_state"/>
   <arg name="TOPIC_ARM_COMMAND" default="/cartesian_velocity_controller/command_cart_vel"/>
-  <arg name="TOPIC_WRENCH_STATE" default="/wrench_fake"/>
+  <arg name="TOPIC_WRENCH_STATE" default="/wrench"/>
 
 
   <param name="topic_arm_state" value="$(arg TOPIC_ARM_STATE)"/>

control_algorithms/admittance/src/Admittance/Admittance.cpp

@@ -57,6 +57,9 @@ Admittance::Admittance(ros::NodeHandle &n,
   base_world_ready_ = false;
   world_arm_ready_ = false;
 
+  force_x_pre = 0;
+  force_y_pre = 0;
+  force_z_pre = 0;
   wait_for_transformations();
 }
 
@@ -160,22 +163,28 @@ void Admittance::state_wrench_callback(
     //                     msg->wrench.torque.z,
     //                     msg->wrench.torque.y,
     //                     msg->wrench.torque.x;
-    float force_thres_lower_limit_ = 3;
+    float force_thres_lower_limit_ = 50;
     float force_thres_upper_limit_ = 100;
     if(fabs(wrench_ft_frame(0)) < force_thres_lower_limit_ || fabs(wrench_ft_frame(0)) > force_thres_upper_limit_){wrench_ft_frame(0) = 0;}
     else{
       if(wrench_ft_frame(0) > 0){wrench_ft_frame(0) -= force_thres_lower_limit_;}
       else{wrench_ft_frame(0) += force_thres_lower_limit_;}
+      wrench_ft_frame(0) = (1 - 0.2)*force_x_pre + 0.2*wrench_ft_frame(0);
+      force_x_pre = wrench_ft_frame(0);
     }
     if(fabs(wrench_ft_frame(1)) < force_thres_lower_limit_ || fabs(wrench_ft_frame(1)) > force_thres_upper_limit_){wrench_ft_frame(1) = 0;}
     else{
       if(wrench_ft_frame(1) > 0){wrench_ft_frame(1) -= force_thres_lower_limit_;}
       else{wrench_ft_frame(1) += force_thres_lower_limit_;}
+      wrench_ft_frame(1) = (1 - 0.2)*force_y_pre + 0.2*wrench_ft_frame(1);
+      force_y_pre = wrench_ft_frame(1);
     }
     if(fabs(wrench_ft_frame(2)) < force_thres_lower_limit_ || fabs(wrench_ft_frame(2)) > force_thres_upper_limit_){wrench_ft_frame(2) = 0;}
     else{
       if(wrench_ft_frame(2) > 0){wrench_ft_frame(2) -= force_thres_lower_limit_;}
       else{wrench_ft_frame(2) += force_thres_lower_limit_;}
+      wrench_ft_frame(2) = (1 - 0.2)*force_z_pre + 0.2*wrench_ft_frame(2);
+      force_z_pre = wrench_ft_frame(2);
     }
     get_rotation_matrix(rotation_ft_base, listener_ft_, base_link_, end_link_);
     wrench_external_ <<  rotation_ft_base * wrench_ft_frame;
@@ -194,19 +203,19 @@ void Admittance::send_commands_to_robot() {
 
   // }
   geometry_msgs::Twist arm_twist_cmd;
-  // arm_twist_cmd.linear.x  = arm_desired_twist_adm_(0)*0.1;
-  // arm_twist_cmd.linear.y  = arm_desired_twist_adm_(1)*0.1;
-  // arm_twist_cmd.linear.z  = arm_desired_twist_adm_(2)*0.1;
-  // arm_twist_cmd.angular.x = arm_desired_twist_adm_(3)*0.1;
-  // arm_twist_cmd.angular.y = arm_desired_twist_adm_(4)*0.1;
-  // arm_twist_cmd.angular.z = arm_desired_twist_adm_(5)*0.1;
-
-  arm_twist_cmd.linear.x  = arm_desired_twist_adm_(0);
-  arm_twist_cmd.linear.y  = arm_desired_twist_adm_(1);
-  arm_twist_cmd.linear.z  = arm_desired_twist_adm_(2);
-  arm_twist_cmd.angular.x = arm_desired_twist_adm_(3);
-  arm_twist_cmd.angular.y = arm_desired_twist_adm_(4);
-  arm_twist_cmd.angular.z = arm_desired_twist_adm_(5);
+  arm_twist_cmd.linear.x  = arm_desired_twist_adm_(0)*0.3;
+  arm_twist_cmd.linear.y  = arm_desired_twist_adm_(1)*0.3;
+  arm_twist_cmd.linear.z  = arm_desired_twist_adm_(2)*0.3;
+  arm_twist_cmd.angular.x = arm_desired_twist_adm_(3)*0.3;
+  arm_twist_cmd.angular.y = arm_desired_twist_adm_(4)*0.3;
+  arm_twist_cmd.angular.z = arm_desired_twist_adm_(5)*0.3;
+
+  // arm_twist_cmd.linear.x  = arm_desired_twist_adm_(0);
+  // arm_twist_cmd.linear.y  = arm_desired_twist_adm_(1);
+  // arm_twist_cmd.linear.z  = arm_desired_twist_adm_(2);
+  // arm_twist_cmd.angular.x = arm_desired_twist_adm_(3);
+  // arm_twist_cmd.angular.y = arm_desired_twist_adm_(4);
+  // arm_twist_cmd.angular.z = arm_desired_twist_adm_(5);
   pub_arm_cmd_.publish(arm_twist_cmd);
 }
 

robots/universal_robot/Universal_Robots_ROS_Driver/ur_calibration/etc/ur5_robot_calibration.yaml

@@ -0,0 +1,44 @@
+kinematics:
+  shoulder:
+    x: 0
+    y: 0
+    z: 0.0893845921254435
+    roll: -0
+    pitch: 0
+    yaw: -4.025317370723778e-05
+  upper_arm:
+    x: 0.0002730898695257202
+    y: 0
+    z: 0
+    roll: 1.570676304469689
+    pitch: 0
+    yaw: -6.698925921262003e-05
+  forearm:
+    x: -0.4253074657852846
+    y: 0
+    z: 0
+    roll: 3.14127765046387
+    pitch: 3.140643712844152
+    yaw: -3.14152183526151
+  wrist_1:
+    x: -0.3919497966358387
+    y: -4.373208522374923e-05
+    z: 0.1110620792638219
+    roll: 0.0003937625088007036
+    pitch: 0.001706084248486594
+    yaw: -6.635982507371374e-05
+  wrist_2:
+    x: 5.221674115481441e-05
+    y: -0.09487233979620983
+    z: 5.011987956616177e-05
+    roll: 1.570268039255966
+    pitch: 0
+    yaw: 4.690559642710078e-05
+  wrist_3:
+    x: 4.885022309742988e-05
+    y: 0.08232249904287943
+    z: 6.251163027391742e-06
+    roll: 1.5708722618441
+    pitch: 3.141592653589793
+    yaw: -3.141585431863596
+  hash: calib_8481515986494097408

robots/universal_robot/Universal_Robots_ROS_Driver/ur_robot_driver/config/ur5_controllers.yaml

@@ -125,6 +125,12 @@ vel_joint_traj_controller:
    state_publish_rate: *loop_hz
    action_monitor_rate: 20
 
+cartesian_velocity_controller:
+   type: cartesian_controller/CartesianVelocityController
+   publish_rate: 125
+   root_name: base_link
+   tip_name: wrist_3_link
+
 # Pass an array of joint velocities directly to the joints
 joint_group_vel_controller:
    type: velocity_controllers/JointGroupVelocityController

robots/universal_robot/Universal_Robots_ROS_Driver/ur_robot_driver/launch/ur5_bringup.launch

@@ -7,8 +7,9 @@
   <arg name="script_sender_port" default="50002" doc="The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately."/>
   <arg name="trajectory_port" default="50003" doc="Port that will be opened by the driver to allow trajectory forwarding."/>
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
-  <arg name="controllers" default="joint_state_controller scaled_pos_joint_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
-  <arg name="stopped_controllers" default="pos_joint_traj_controller joint_group_vel_controller" doc="Controllers that are initally loaded, but not started."/>
+  <!-- <arg name="controllers" default="joint_state_controller scaled_pos_joint_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/> -->
+  <arg name="controllers" default="cartesian_velocity_controller joint_state_controller  force_torque_sensor_controller" doc="Controllers that are activated by default."/>
+
   <arg name="controller_config_file" default="$(find ur_robot_driver)/config/ur5_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/>
   <arg name="robot_description_file" default="$(find ur_description)/launch/load_ur5.launch" doc="Robot description launch file."/>
   <arg name="kinematics_config" default="$(find ur_description)/config/ur5/default_kinematics.yaml" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description."/>