added franka control module

franka/caller.py

@@ -0,0 +1,57 @@
+import subprocess
+import time
+
+while True:
+    testing = input("Is this program being tested [N/y]: ")
+    if testing == '' or testing.lower() == 'n':
+        testing = False
+        break
+    elif testing.lower() == 'y':
+        testing = True
+        break
+    else:
+        print("Invalid response.")
+print("Testing mode: ", testing)
+
+while True:
+    direction = input("Type 0 for forward, 1 for back: ")
+    if direction in ['0', '1']:
+        break
+    else:
+        print("direction was not understood")
+
+print("Direction is: ", direction)
+
+dx = '0'
+
+if direction == '0':
+    dx = '0.05'
+elif direction == '1':
+    dx = '-0.05'
+
+dy = '0'
+dz = '0'
+
+print("dx: ", dx)
+print("dy: ", dy)
+print("dz: ", dz)
+
+program = './franka_move_to_relative'
+ip_address = '192.168.0.88'
+
+print("Program being run is: ", program)
+print("IP Address of robot: ", ip_address)
+
+command = [program, ip_address, dx, dy, dz]
+command_str = " ".join(command)
+
+print("Command being called: ", command)
+
+if testing:
+    print("Running the actual FRANKA code in 3 seconds!")
+    time.sleep(3)
+    print("Running it now!")
+    returncode = subprocess.call(command)
+    print("Python reads the return code as: ", returncode)
+    if returncode != 0:
+        print("Python has registered a problem with the subprocess.")

franka/franka_get_current_position.cpp

@@ -0,0 +1,49 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <cmath>
+#include <iostream>
+
+#include <franka/exception.h>
+#include <franka/robot.h>
+
+#include <unistd.h>
+
+
+
+/**
+ * @example generate_cartesian_velocity_motion.cpp
+ * An example showing how to generate a Cartesian velocity motion.
+ *
+ * @warning Before executing this example, make sure there is enough space in front of the robot.
+ */
+
+int main(int argc, char** argv) {
+  if (argc != 2) {
+    std::cerr << "Usage: ./generate_cartesian_velocity_motion <robot-hostname>" << std::endl;
+    return -1;
+  }
+  try {
+
+    std::cout.precision(3); 
+
+    franka::Robot robot(argv[1]);
+
+    size_t count = 0;
+    robot.read([&count](const franka::RobotState& robot_state) {
+
+      auto state_pose = robot_state.O_T_EE_d;
+      std::array<double, 16> current_pose = state_pose;
+
+      std::cout << "current position x y z: " << current_pose[12] << "  " << current_pose[13] << "  " << current_pose[14] << std::endl; 
+      sleep(1); 
+
+      return 1; 
+    });
+
+  } catch (const franka::Exception& e) {
+    std::cout << e.what() << std::endl;
+    return -1;
+  }
+
+  return 0;
+}

franka/franka_move_to_absolute.cpp

@@ -0,0 +1,131 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <cmath>
+#include <iostream>
+
+#include <franka/duration.h>
+#include <franka/exception.h>
+#include <franka/model.h>
+#include <franka/robot.h>
+
+
+/**
+ * @example generate_cartesian_velocity_motion.cpp
+ * An example showing how to generate a Cartesian velocity motion.
+ *
+ * @warning Before executing this example, make sure there is enough space in front of the robot.
+ */
+
+int main(int argc, char** argv) {
+  if (argc != 5) {
+    std::cerr << "Usage: ./generate_cartesian_velocity_motion <robot-hostname> delta_x delta_y delta_z" << std::endl;
+    return -1;
+  }
+  try {
+
+    franka::Robot robot(argv[1]);
+
+    // Set additional parameters always before the control loop, NEVER in the control loop!
+    // Set the joint impedance.
+    // robot.setJointImpedance({{300, 300, 300, 250, 250, 200, 200}});
+
+    // Set the collision behavior.
+    std::array<double, 7> lower_torque_thresholds_nominal{
+        {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}};
+    std::array<double, 7> upper_torque_thresholds_nominal{
+        {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
+    std::array<double, 7> lower_torque_thresholds_acceleration{
+        {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}};
+    std::array<double, 7> upper_torque_thresholds_acceleration{
+        {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
+    std::array<double, 6> lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
+    std::array<double, 6> upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
+    std::array<double, 6> lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
+    std::array<double, 6> upper_force_thresholds_acceleration{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
+    robot.setCollisionBehavior(
+        lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration,
+        lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
+        lower_force_thresholds_acceleration, upper_force_thresholds_acceleration,
+        lower_force_thresholds_nominal, upper_force_thresholds_nominal);
+
+    double time = 0.0;
+
+    double speed = 0.10; 
+
+    auto initial_pose = robot.readOnce().O_T_EE_d;
+    std::array<double, 16> current_pose = initial_pose;
+
+    double target_x = atof(argv[2]); 
+    double target_y = atof(argv[3]); 
+    double target_z = atof(argv[4]); 
+
+
+    robot.control([=, &time](const franka::RobotState& robot_state,
+                             franka::Duration time_step) -> franka::CartesianVelocities {
+
+      double vel_x = 0.0; 
+      double vel_y = 0.0; 
+      double vel_z = 0.0; 
+
+      static double old_vel_x = 0.0; 
+      static double old_vel_y = 0.0; 
+      static double old_vel_z = 0.0; 
+
+      time += time_step.toSec(); 
+
+      auto state_pose = robot_state.O_T_EE_d;
+      std::array<double, 16> current_pose = state_pose;
+
+
+      double cur_x = current_pose[12]; 
+      double cur_y = current_pose[13];
+      double cur_z = current_pose[14]; 
+
+      double vec_x = target_x - cur_x;
+      double vec_y = target_y - cur_y;
+      double vec_z = target_z - cur_z; 
+
+      double l2_norm = sqrt(vec_x*vec_x + vec_y*vec_y + vec_z*vec_z); 
+
+      if (l2_norm < 0.02) {
+          vel_x = 0.9*old_vel_x;
+          vel_y = 0.9*old_vel_y; 
+          vel_z = 0.9*old_vel_z; 
+      }
+      else {
+        vel_x = speed*(vec_x / l2_norm);
+        vel_y = speed*(vec_y / l2_norm);
+        vel_z = speed*(vec_z / l2_norm); 
+      }
+
+      vel_x = 0.99*old_vel_x + 0.01*vel_x;
+      vel_y = 0.99*old_vel_y + 0.01*vel_y;
+      vel_z = 0.99*old_vel_z + 0.01*vel_z;
+
+      old_vel_x = vel_x;
+      old_vel_y = vel_y;
+      old_vel_z = vel_z;
+
+      franka::CartesianVelocities output = {{vel_x, vel_y, vel_z, 0.0, 0.0, 0.0}};
+
+      double vel_norm = sqrt(vel_x*vel_x + vel_y*vel_y + vel_z*vel_z); 
+      if (vel_norm < 0.001) {
+        // stop program when target reached
+        std::cout << std::endl << "Finished motion, shutting down..." << std::endl << std::flush;
+        franka::CartesianVelocities output = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+        return franka::MotionFinished(output);
+      }
+      // franka::CartesianVelocities output = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+/*      if (time >= 2 * time_max) {
+        std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
+        return franka::MotionFinished(output);
+      }*/
+      return output;
+    });
+  } catch (const franka::Exception& e) {
+    std::cout << e.what() << std::endl;
+    return -1;
+  }
+
+  return 0;
+}