Merge pull request #3 from nebbles/franka_control

Franka control update with end effector position

franka/franka_control.py

@@ -62,6 +62,32 @@ def get_joint_positions(self):
 
         return converted_list
 
+    def get_end_effector_pos(self):
+        """Gets current x,y,z positions for Franka Arm end effector.
+
+        Returns list of x,y,z values.
+        """
+
+        program = './franka_get_current_position'  # set executable to be used
+        command = [program, self.ip_address]
+        command_str = " ".join(command)
+
+        if self.debug:
+            print("Working directory: ", self.path)
+            print("Program: ", program)
+            print("IP Address of robot: ", self.ip_address)
+            print("Command being called: ", command_str)
+            print("Running FRANKA code...")
+
+        process = subprocess.Popen(command, stdout=subprocess.PIPE)
+        out, err = process.communicate()  # this will block until received
+        decoded_output = out.decode("utf-8")
+
+        import ast
+        converted_list = ast.literal_eval(decoded_output)
+
+        return converted_list
+
     def move_relative(self, dx=0.0, dy=0.0, dz=0.0):
         """Moves Franka Arm relative to its current position.
 
@@ -92,6 +118,8 @@ def move_relative(self, dx=0.0, dy=0.0, dz=0.0):
             print("Command being called: ", command_str)
             print("Running FRANKA code...")
 
+        # TODO: option to suppress output
+        # TODO: catch errors returned by the subprocess
         return_code = subprocess.call(command, cwd=self.path)
 
         if return_code == 0:
@@ -132,6 +160,8 @@ def move_absolute(self, coordinates):
             print("Command being called: ", command_str)
             print("Running FRANKA code...")
 
+        # TODO: option to suppress output
+        # TODO: catch errors returned by the subprocess
         return_code = subprocess.call(command, cwd=self.path)
 
         if return_code == 0:
@@ -204,10 +234,10 @@ def test_motion():
         print("Command being called: ", command_str)
 
 
-def test_position():
+def test_joints():
     """Used to test if position reporting is working from Arm.
 
-    To use this test, add the -p or --position-test flag to the command line.
+    To use this test, add the -j or --joint-test flag to the command line.
     """
     arm = FrankaControl(debug_flag=True)
     while True:
@@ -217,18 +247,35 @@ def test_position():
             "%1.0f   %1.0f   %1.0f   %1.0f   %5.3f   %1.0f" % tuple(matrix[0]))
 
 
+def test_position():
+    """Used to test if the Franka Arm is reporting the position of its end effector.
+
+    To use this test, add the -p or --position-test flag to the command line.
+    """
+    arm = FrankaControl(debug_flag=True)
+    pos = arm.get_end_effector_pos()
+    print("End effector position:")
+    print("X: ", pos[0])
+    print("Y: ", pos[1])
+    print("Z: ", pos[2])
+
+
 if __name__ == '__main__':
     parser = argparse.ArgumentParser(description='Control Franka Arm with Python.')
     parser.add_argument('-m', '--motion-test', action='store_true',
                         help='run program in testing motion mode')
     parser.add_argument('-p', '--position-test', action='store_true',
                         help='run program in testing position readings mode')
+    parser.add_argument('-j', '--joint-test', action='store_true',
+                        help='run program in testing joint readings mode')
 
     args = parser.parse_args()  # Get command line args
 
     if args.motion_test:
         test_motion()
     elif args.position_test:
         test_position()
+    elif args.joint_test:
+        test_joints()
     else:
         print("Try: franka_control.py --help")

franka/franka_get_current_position.cpp

@@ -34,10 +34,11 @@ int main(int argc, char** argv) {
       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); 
+      //std::cout << "current position x y z: " << current_pose[12] << "  " << current_pose[13] << "  " << current_pose[14] << std::endl; 
+      std::cout << "[" << current_pose[12] << "," << current_pose[13] << "," << current_pose[14] << "]" << std::endl; 
+      //sleep(1); 
 
-      return 1; 
+      return 0; 
     });
 
   } catch (const franka::Exception& e) {

franka/generate_cartesian_pose_motion.cpp

@@ -0,0 +1,60 @@
+// 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>
+
+/**
+ * @example generate_cartesian_pose_motion.cpp
+ * An example showing how to generate a Cartesian 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_pose_motion <robot-hostname>" << std::endl;
+    return -1;
+  }
+
+  try {
+    franka::Robot robot(argv[1]);
+
+    // Set additional parameters always before the control loop, NEVER in the control loop!
+    // Set collision behavior.
+    robot.setCollisionBehavior(
+        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
+        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
+        {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
+        {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
+
+    auto initial_pose = robot.readOnce().O_T_EE_d;
+    double radius = 0.3;
+    double time = 0.0;
+    robot.control(
+        [=, &time](const franka::RobotState&, franka::Duration time_step) -> franka::CartesianPose {
+          time += time_step.toSec();
+
+          double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * time));
+          double delta_x = radius * std::sin(angle);
+          double delta_z = radius * (std::cos(angle) - 1);
+
+          std::array<double, 16> new_pose = initial_pose;
+          new_pose[12] += delta_x;
+          new_pose[14] += delta_z;
+
+          if (time >= 10.0) {
+            std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
+            return franka::MotionFinished(new_pose);
+          }
+          return new_pose;
+        });
+  } catch (const franka::Exception& e) {
+    std::cout << e.what() << std::endl;
+    return -1;
+  }
+
+  return 0;
+}

franka/generate_cartesian_velocity_motion.cpp

@@ -0,0 +1,73 @@
+// 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>
+
+/**
+ * @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 {
+    franka::Robot robot(argv[1]);
+
+    // Set additional parameters always before the control loop, NEVER in the control loop!
+    // Set the joint impedance.
+    robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
+
+    // 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_max = 4.0;
+    double v_max = 0.1;
+    double angle = M_PI / 4.0;
+    double time = 0.0;
+    robot.control([=, &time](const franka::RobotState&,
+                             franka::Duration time_step) -> franka::CartesianVelocities {
+      time += time_step.toSec();
+
+      double cycle = std::floor(pow(-1.0, (time - std::fmod(time, time_max)) / time_max));
+      double v = cycle * v_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time));
+      double v_x = std::cos(angle) * v;
+      double v_z = -std::sin(angle) * v;
+
+      franka::CartesianVelocities output = {{v_x, 0.0, v_z, 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;
+}

franka/generate_consecutive_motions.cpp

@@ -0,0 +1,68 @@
+// 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>
+
+/**
+ * @example generate_consecutive_motions.cpp
+ * An example showing how to execute consecutive motions with error recovery.
+ *
+ * @warning Before executing this example, make sure there is enough space in front and to the side
+ * of the robot.
+ */
+
+int main(int argc, char** argv) {
+  if (argc != 2) {
+    std::cerr << "Usage: ./generate_consecutive_motions <robot-hostname>" << std::endl;
+    return -1;
+  }
+
+  try {
+    franka::Robot robot(argv[1]);
+
+    // Set additional parameters always before the control loop, NEVER in the control loop!
+    // Set collision behavior.
+    robot.setCollisionBehavior(
+        {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}},
+        {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}},
+        {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}},
+        {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}});
+
+    for (int i = 0; i < 5; i++) {
+      std::cout << "Executing motion." << std::endl;
+      try {
+        double time_max = 4.0;
+        double omega_max = 0.2;
+        double time = 0.0;
+        robot.control([=, &time](const franka::RobotState&,
+                                 franka::Duration time_step) -> franka::JointVelocities {
+          time += time_step.toSec();
+
+          double cycle = std::floor(std::pow(-1.0, (time - std::fmod(time, time_max)) / time_max));
+          double omega = cycle * omega_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time));
+
+          franka::JointVelocities velocities = {{0.0, 0.0, omega, 0.0, 0.0, 0.0, 0.0}};
+          if (time >= 2 * time_max) {
+            std::cout << std::endl << "Finished motion." << std::endl;
+            return franka::MotionFinished(velocities);
+          }
+          return velocities;
+        });
+      } catch (const franka::ControlException& e) {
+        std::cout << e.what() << std::endl;
+        std::cout << "Running error recovery..." << std::endl;
+        robot.automaticErrorRecovery();
+      }
+    }
+  } catch (const franka::Exception& e) {
+    std::cout << e.what() << std::endl;
+    return -1;
+  }
+
+  std::cout << "Finished." << std::endl;
+
+  return 0;
+}

franka/generate_joint_position_motion.cpp

@@ -0,0 +1,58 @@
+// 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>
+
+/**
+ * @example generate_joint_position_motion.cpp
+ * An example showing how to generate a joint position 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_joint_position_motion <robot-hostname>" << std::endl;
+    return -1;
+  }
+
+  try {
+    franka::Robot robot(argv[1]);
+
+    // Set additional parameters always before the control loop, NEVER in the control loop!
+    // Set collision behavior.
+    robot.setCollisionBehavior(
+        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
+        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
+        {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
+        {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
+
+    auto initial_position = robot.readOnce().q_d;
+    double time = 0.0;
+    robot.control([=, &time](const franka::RobotState&,
+                             franka::Duration time_step) -> franka::JointPositions {
+      time += time_step.toSec();
+
+      double delta_angle = M_PI / 8 * (1 - std::cos(M_PI / 5.0 * time));
+
+      franka::JointPositions output = {{initial_position[0], initial_position[1],
+                                        initial_position[2], initial_position[3] + delta_angle,
+                                        initial_position[4] + delta_angle, initial_position[5],
+                                        initial_position[6] + delta_angle}};
+
+      if (time >= 10.0) {
+        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;
+}