From 62aef210e4af6c84920bbb7f28f2f535683fd09d Mon Sep 17 00:00:00 2001
From: andrea <andrea@nuc>
Date: Fri, 23 Jul 2021 16:35:04 -0400
Subject: [PATCH 1/7] basic cartesian motion for each panda arm

---
 arm_robots/scripts/tutorial/panda_basic_motion.py | 3 +++
 arm_robots/src/arm_robots/panda.py                | 5 ++++-
 2 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/arm_robots/scripts/tutorial/panda_basic_motion.py b/arm_robots/scripts/tutorial/panda_basic_motion.py
index d26f3bc..cc9e1b6 100755
--- a/arm_robots/scripts/tutorial/panda_basic_motion.py
+++ b/arm_robots/scripts/tutorial/panda_basic_motion.py
@@ -7,5 +7,8 @@
     rospy.init_node('panda_motion')
     panda = Panda()
     panda.connect()
+    panda.set_execute(False) # this will stop the robot from actually executing a path, good for testing
+    # panda.plan_to_position_cartesian(panda.nebula_arm, panda.nebula_wrist, target_position=[0.04, 0.2867, 1.17206])
+    # panda.plan_to_position_cartesian(panda.rocket_arm, panda.rocket_wrist, target_position=[-0.013, -0.04, 1.16967])
 
     # TODO: Reference med_motion.py for examples.
\ No newline at end of file
diff --git a/arm_robots/src/arm_robots/panda.py b/arm_robots/src/arm_robots/panda.py
index 2ecb8a8..5e60956 100755
--- a/arm_robots/src/arm_robots/panda.py
+++ b/arm_robots/src/arm_robots/panda.py
@@ -15,7 +15,10 @@ def __init__(self, robot_namespace: str = 'combined_panda', force_trigger: float
                                     arms_controller_name='effort_joint_trajectory_controller',
                                     force_trigger=force_trigger,
                                     **kwargs)
-
+        self.nebula_arm = "nebula_arm"
+        self.rocket_arm = "rocket_arm"
+        self.nebula_wrist = "panda_1_link8"
+        self.rocket_wrist = "panda_2_link8"
     def send_joint_command(self, joint_names: List[str], trajectory_point: JointTrajectoryPoint) -> Tuple[bool, str]:
         # TODO: Fill in to send set point to controller.
         pass

From 55e26c8ad94bda3430d5c46f87842169428ac3e4 Mon Sep 17 00:00:00 2001
From: andrea <andrea@nuc>
Date: Fri, 5 Nov 2021 13:02:01 -0400
Subject: [PATCH 2/7] added gripper interface, error clearing

---
 .../scripts/tutorial/panda_basic_motion.py    |  26 +++-
 arm_robots/src/arm_robots/panda.py            | 113 +++++++++++++++++-
 arm_robots/src/arm_robots/robot.py            |   6 +-
 3 files changed, 136 insertions(+), 9 deletions(-)

diff --git a/arm_robots/scripts/tutorial/panda_basic_motion.py b/arm_robots/scripts/tutorial/panda_basic_motion.py
index cc9e1b6..456358d 100755
--- a/arm_robots/scripts/tutorial/panda_basic_motion.py
+++ b/arm_robots/scripts/tutorial/panda_basic_motion.py
@@ -1,14 +1,38 @@
 #! /usr/bin/env python
 
 import rospy
+import pdb
+import time
+import numpy as np
 from arm_robots.panda import Panda
 
+
 if __name__ == '__main__':
     rospy.init_node('panda_motion')
     panda = Panda()
     panda.connect()
+    pdb.set_trace()
     panda.set_execute(False) # this will stop the robot from actually executing a path, good for testing
     # panda.plan_to_position_cartesian(panda.nebula_arm, panda.nebula_wrist, target_position=[0.04, 0.2867, 1.17206])
     # panda.plan_to_position_cartesian(panda.rocket_arm, panda.rocket_wrist, target_position=[-0.013, -0.04, 1.16967])
-
+    # poking_approach_pose_1 = [0.0, 0.0, 1.07, np.radians(135), np.radians(-90), np.radians(225)]
+    # poking_target_pose_1 = [-0.039, 0.0, 1.07, np.radians(135), np.radians(-90), np.radians(225)]
+    # poking_approach_pose_2 = [0.0, 0.015, 1.085, np.radians(135), np.radians(-90), np.radians(225)]
+    # poking_target_pose_2 = [-0.039, 0.015, 1.085, np.radians(135), np.radians(-90), np.radians(225)]
+    # poking_approach_pose_3 = [0.0, 0.015, 1.055, np.radians(135), np.radians(-90), np.radians(225)]
+    # poking_target_pose_3 = [-0.039, 0.015, 1.055, np.radians(135), np.radians(-90), np.radians(225)]
+    # poking_approach_pose_4 = [0.0, -0.015, 1.055, np.radians(135), np.radians(-90), np.radians(225)]
+    # poking_target_pose_4 = [-0.039, -0.015, 1.055, np.radians(135), np.radians(-90), np.radians(225)]
+    # poking_approach_pose_5 = [0.0, -0.015, 1.085, np.radians(135), np.radians(-90), np.radians(225)]
+    # poking_target_pose_5 = [-0.039, -0.015, 1.085, np.radians(135), np.radians(-90), np.radians(225)]
+    # pdb.set_trace()
+    # poking_approach_pose_1 = [0.29320, 0.00387, 0.75, 1.0, 0.0, 0.0, 0.0]
+    # poking_target_pose_1 = [0.29320, 0.00387, 0.723] #, 1.0, 0.0, 0.0, 0.0]
+    # time.sleep(1)
+    # panda.plan_to_pose(panda.rocket_arm, "rocket_EE", poking_approach_pose_1, "world")
+    # time.sleep(3)
+    # panda.plan_to_position_cartesian(panda.rocket_arm, "rocket_EE", poking_target_pose_1, 0.001)
+    # time.sleep(3)
+    # panda.plan_to_pose(panda.rocket_arm, "rocket_EE", poking_approach_pose_1, "world")
+    # time.sleep(3)
     # TODO: Reference med_motion.py for examples.
\ No newline at end of file
diff --git a/arm_robots/src/arm_robots/panda.py b/arm_robots/src/arm_robots/panda.py
index 5e60956..ce52407 100755
--- a/arm_robots/src/arm_robots/panda.py
+++ b/arm_robots/src/arm_robots/panda.py
@@ -1,10 +1,16 @@
 #! /usr/bin/env python
+from copy import Error
 from rosgraph.names import ns_join
 from typing import List, Tuple
 
 from arm_robots.robot import MoveitEnabledRobot
 from trajectory_msgs.msg import JointTrajectoryPoint
-
+from sensor_msgs.msg import JointState
+from franka_msgs.msg import ErrorRecoveryAction, ErrorRecoveryGoal, FrankaState
+from franka_gripper.msg import GraspAction, GraspGoal, MoveAction, MoveGoal, HomingAction, HomingGoal, StopAction, StopGoal
+import rospy
+import actionlib
+import pdb
 
 class Panda(MoveitEnabledRobot):
 
@@ -16,13 +22,110 @@ def __init__(self, robot_namespace: str = 'combined_panda', force_trigger: float
                                     force_trigger=force_trigger,
                                     **kwargs)
         self.nebula_arm = "nebula_arm"
+        self.nebula_id = "panda_1"
         self.rocket_arm = "rocket_arm"
+        self.rocket_id = "panda_2"
         self.nebula_wrist = "panda_1_link8"
         self.rocket_wrist = "panda_2_link8"
-    def send_joint_command(self, joint_names: List[str], trajectory_point: JointTrajectoryPoint) -> Tuple[bool, str]:
-        # TODO: Fill in to send set point to controller.
-        pass
+        self.nebula_gripper = PandaGripper(self.robot_namespace, self.nebula_id)
+        self.rocket_gripper = PandaGripper(self.robot_namespace, self.rocket_id)
+        rospy.Subscriber("/"+self.robot_namespace+"/"+self.nebula_id+"_state_controller/franka_states", FrankaState, self.nebula_error_cb)
+        rospy.Subscriber("/"+self.robot_namespace+"/"+self.rocket_id+"_state_controller/franka_states", FrankaState, self.rocket_error_cb)
+
+    def open_nebula_gripper(self):
+        self.nebula_gripper.move(self.nebula_gripper.MAX_WIDTH)
+
+    def close_nebula_gripper(self):
+        self.nebula_gripper.move(self.nebula_gripper.MIN_WIDTH)
+
+    def open_rocket_gripper(self):
+        self.rocket_gripper.move(self.rocket_gripper.MAX_WIDTH)
+
+    def close_rocket_gripper(self):
+        self.rocket_gripper.move(self.rocket_gripper.MIN_WIDTH)
+
+    def nebula_error_cb(self, data):
+        if data.current_errors.communication_constraints_violation or data.last_motion_errors.communication_constraints_violation:
+            self.clear_errors(wait_for_result=True)
+
+    def rocket_error_cb(self, data):
+        if data.current_errors.communication_constraints_violation or data.last_motion_errors.communication_constraints_violation:
+            self.clear_errors(wait_for_result=True)
 
-    # TODO: Add gripper helpers.
+    def clear_errors(self, wait_for_result=False):
+        self.error_client = actionlib.SimpleActionClient("/"+self.robot_namespace+"/error_recovery", ErrorRecoveryAction)
+        self.error_client.wait_for_server()
+        goal = ErrorRecoveryGoal()
+        self.error_client.send_goal(goal)
+        if wait_for_result:
+            result = self.error_client.wait_for_result(rospy.Duration(15.))
+            return result
+        return True
 
     # TODO: Add control mode setter/getter.
+
+class PandaGripper:
+    def __init__(self, robot_ns, arm_id):
+        self.gripper_ns = "/"+robot_ns+"/"+arm_id+"/franka_gripper/"
+        self.grasp_client = actionlib.SimpleActionClient(self.gripper_ns+"grasp", GraspAction)
+        self.move_client = actionlib.SimpleActionClient(self.gripper_ns+"move", MoveAction)
+        self.homing_client = actionlib.SimpleActionClient(self.gripper_ns+"homing", HomingAction)
+        self.stop_client = actionlib.SimpleActionClient(self.gripper_ns+"stop", StopAction)
+        self.grasp_client.wait_for_server()
+        self.move_client.wait_for_server()
+        self.homing_client.wait_for_server()
+        self.stop_client.wait_for_server()
+        self.gripper_width = None
+        rospy.Subscriber(self.gripper_ns+"joint_states", JointState, self.gripper_cb)
+        self.MIN_FORCE = 0.05
+        self.MAX_FORCE = 50 # documentation says up to 70N is possible as continuous force
+        self.MIN_WIDTH = 0.0
+        self.MAX_WIDTH = 0.08
+        self.DEFAULT_EPSILON = 0.005
+        self.DEFAULT_SPEED = 0.02
+        self.DEFAULT_FORCE = 10
+        # TODO: add to cfg file
+
+    def gripper_cb(self, data):
+        self.gripper_width = data.position[0] + data.position[1]
+
+    def grasp(self, width, speed=None, epsilon_outer=None, epsilon_inner=None, force=None, wait_for_result=False):
+        if width > self.gripper_width:
+            self.move(self.MAX_WIDTH, wait_for_result=True)
+        goal = GraspGoal()
+        goal.width = width
+        goal.epsilon.outer = self.DEFAULT_EPSILON if not epsilon_outer else epsilon_outer
+        goal.epsilon.inner = self.DEFAULT_EPSILON if not epsilon_inner else epsilon_inner
+        goal.speed = self.DEFAULT_SPEED if not speed else speed
+        goal.force = self.DEFAULT_FORCE if not force else force
+        self.grasp_client.send_goal(goal)
+        if wait_for_result:
+            result = self.grasp_client.wait_for_result(rospy.Duration(15.))
+            return result
+        return True
+
+    def move(self, width, speed=None, wait_for_result=False):
+        goal = MoveGoal()
+        goal.width = width
+        goal.speed = self.DEFAULT_SPEED if not speed else speed
+        self.move_client.send_goal(goal)
+        if wait_for_result:
+            result = self.move_client.wait_for_result(rospy.Duration(15.))
+            return result
+        return True
+
+    def homing(self, wait_for_result=True):
+        goal = HomingGoal()
+        self.homing_client.send_goal(goal)
+        if wait_for_result:
+            result = self.homing_client.wait_for_result(rospy.Duration(15.))
+            return result
+        return True
+
+    def stop(self, wait_for_result=False):
+        goal = StopGoal()
+        self.stop_client.send_goal(goal)
+        if wait_for_result:
+            result = self.stop_client.wait_for_result(rospy.Duration(15.))
+            return result
+        return True
\ No newline at end of file
diff --git a/arm_robots/src/arm_robots/robot.py b/arm_robots/src/arm_robots/robot.py
index 3f8c4b5..84daaf6 100644
--- a/arm_robots/src/arm_robots/robot.py
+++ b/arm_robots/src/arm_robots/robot.py
@@ -191,7 +191,7 @@ def plan_to_pose(self, group_name, ee_link_name, target_pose, frame_id: str = 'r
         move_group.set_goal_orientation_tolerance(0.02)
 
         if self.display_goals:
-            self.display_goal_pose(target_pose_stamped.pose)
+            self.display_goal_pose(target_pose_stamped.pose, frame_id, ee_link_name)
 
         planning_result = PlanningResult(move_group.plan())
         if self.raise_on_failure and not planning_result.success:
@@ -515,8 +515,8 @@ def display_goal_position(self, point: Point):
         m.pose.orientation.w = 1
         self.display_goal_position_pub.publish(m)
 
-    def display_goal_pose(self, pose: Pose):
-        self.tf_wrapper.send_transform_from_pose_msg(pose, 'robot_root', 'arm_robots_goal')
+    def display_goal_pose(self, pose: Pose, root='robot_root', goal='arm_robots_goal'):
+        self.tf_wrapper.send_transform_from_pose_msg(pose, root, goal)
 
     def get_state(self, group_name: str = None):
         robot_state = RobotState()

From 287fdfddb3a2e333f7ba97418b12fd54ac373669 Mon Sep 17 00:00:00 2001
From: Andrea <asipos@umich.edu>
Date: Tue, 22 Mar 2022 10:14:46 -0400
Subject: [PATCH 3/7] fixed comments from pr

---
 .../scripts/tutorial/panda_basic_motion.py    |  9 +--
 arm_robots/src/arm_robots/panda.py            | 57 ++++++++++---------
 2 files changed, 31 insertions(+), 35 deletions(-)

diff --git a/arm_robots/scripts/tutorial/panda_basic_motion.py b/arm_robots/scripts/tutorial/panda_basic_motion.py
index 456358d..ab67936 100755
--- a/arm_robots/scripts/tutorial/panda_basic_motion.py
+++ b/arm_robots/scripts/tutorial/panda_basic_motion.py
@@ -1,18 +1,13 @@
 #! /usr/bin/env python
 
 import rospy
-import pdb
-import time
-import numpy as np
 from arm_robots.panda import Panda
 
-
 if __name__ == '__main__':
     rospy.init_node('panda_motion')
     panda = Panda()
     panda.connect()
-    pdb.set_trace()
-    panda.set_execute(False) # this will stop the robot from actually executing a path, good for testing
+    panda.set_execute(False)  # this will stop the robot from actually executing a path, good for testing
     # panda.plan_to_position_cartesian(panda.nebula_arm, panda.nebula_wrist, target_position=[0.04, 0.2867, 1.17206])
     # panda.plan_to_position_cartesian(panda.rocket_arm, panda.rocket_wrist, target_position=[-0.013, -0.04, 1.16967])
     # poking_approach_pose_1 = [0.0, 0.0, 1.07, np.radians(135), np.radians(-90), np.radians(225)]
@@ -35,4 +30,4 @@
     # time.sleep(3)
     # panda.plan_to_pose(panda.rocket_arm, "rocket_EE", poking_approach_pose_1, "world")
     # time.sleep(3)
-    # TODO: Reference med_motion.py for examples.
\ No newline at end of file
+    # TODO: Reference med_motion.py for examples.
diff --git a/arm_robots/src/arm_robots/panda.py b/arm_robots/src/arm_robots/panda.py
index ce52407..7e1d9ba 100755
--- a/arm_robots/src/arm_robots/panda.py
+++ b/arm_robots/src/arm_robots/panda.py
@@ -1,16 +1,13 @@
 #! /usr/bin/env python
-from copy import Error
-from rosgraph.names import ns_join
-from typing import List, Tuple
-
+import actionlib
+import rospy
 from arm_robots.robot import MoveitEnabledRobot
-from trajectory_msgs.msg import JointTrajectoryPoint
-from sensor_msgs.msg import JointState
+from franka_gripper.msg import GraspAction, GraspGoal, MoveAction, MoveGoal, HomingAction, HomingGoal, StopAction, \
+    StopGoal
 from franka_msgs.msg import ErrorRecoveryAction, ErrorRecoveryGoal, FrankaState
-from franka_gripper.msg import GraspAction, GraspGoal, MoveAction, MoveGoal, HomingAction, HomingGoal, StopAction, StopGoal
-import rospy
-import actionlib
-import pdb
+from rosgraph.names import ns_join
+from sensor_msgs.msg import JointState
+
 
 class Panda(MoveitEnabledRobot):
 
@@ -18,7 +15,7 @@ def __init__(self, robot_namespace: str = 'combined_panda', force_trigger: float
         MoveitEnabledRobot.__init__(self,
                                     robot_namespace=robot_namespace,
                                     robot_description=ns_join(robot_namespace, 'robot_description'),
-                                    arms_controller_name='effort_joint_trajectory_controller',
+                                    arms_controller_name='position_joint_trajectory_controller',
                                     force_trigger=force_trigger,
                                     **kwargs)
         self.nebula_arm = "nebula_arm"
@@ -29,8 +26,13 @@ def __init__(self, robot_namespace: str = 'combined_panda', force_trigger: float
         self.rocket_wrist = "panda_2_link8"
         self.nebula_gripper = PandaGripper(self.robot_namespace, self.nebula_id)
         self.rocket_gripper = PandaGripper(self.robot_namespace, self.rocket_id)
-        rospy.Subscriber("/"+self.robot_namespace+"/"+self.nebula_id+"_state_controller/franka_states", FrankaState, self.nebula_error_cb)
-        rospy.Subscriber("/"+self.robot_namespace+"/"+self.rocket_id+"_state_controller/franka_states", FrankaState, self.rocket_error_cb)
+        rospy.Subscriber(ns_join(self.robot_namespace, f'{self.nebula_id}_state_controller/franka_states'), FrankaState,
+                         self.nebula_error_cb)
+        rospy.Subscriber(ns_join(self.robot_namespace, f'{self.rocket_id}_state_controller/franka_states'), FrankaState,
+                         self.rocket_error_cb)
+        self.error_client = actionlib.SimpleActionClient(ns_join(self.robot_namespace, "error_recovery"),
+                                                         ErrorRecoveryAction)
+        self.error_client.wait_for_server()
 
     def open_nebula_gripper(self):
         self.nebula_gripper.move(self.nebula_gripper.MAX_WIDTH)
@@ -53,32 +55,31 @@ def rocket_error_cb(self, data):
             self.clear_errors(wait_for_result=True)
 
     def clear_errors(self, wait_for_result=False):
-        self.error_client = actionlib.SimpleActionClient("/"+self.robot_namespace+"/error_recovery", ErrorRecoveryAction)
-        self.error_client.wait_for_server()
         goal = ErrorRecoveryGoal()
         self.error_client.send_goal(goal)
         if wait_for_result:
-            result = self.error_client.wait_for_result(rospy.Duration(15.))
+            result = self.error_client.wait_for_result(rospy.Duration(10))
             return result
         return True
 
     # TODO: Add control mode setter/getter.
 
+
 class PandaGripper:
     def __init__(self, robot_ns, arm_id):
-        self.gripper_ns = "/"+robot_ns+"/"+arm_id+"/franka_gripper/"
-        self.grasp_client = actionlib.SimpleActionClient(self.gripper_ns+"grasp", GraspAction)
-        self.move_client = actionlib.SimpleActionClient(self.gripper_ns+"move", MoveAction)
-        self.homing_client = actionlib.SimpleActionClient(self.gripper_ns+"homing", HomingAction)
-        self.stop_client = actionlib.SimpleActionClient(self.gripper_ns+"stop", StopAction)
+        self.gripper_ns = "/" + robot_ns + "/" + arm_id + "/franka_gripper/"
+        self.grasp_client = actionlib.SimpleActionClient(self.gripper_ns + "grasp", GraspAction)
+        self.move_client = actionlib.SimpleActionClient(self.gripper_ns + "move", MoveAction)
+        self.homing_client = actionlib.SimpleActionClient(self.gripper_ns + "homing", HomingAction)
+        self.stop_client = actionlib.SimpleActionClient(self.gripper_ns + "stop", StopAction)
         self.grasp_client.wait_for_server()
         self.move_client.wait_for_server()
         self.homing_client.wait_for_server()
         self.stop_client.wait_for_server()
         self.gripper_width = None
-        rospy.Subscriber(self.gripper_ns+"joint_states", JointState, self.gripper_cb)
+        rospy.Subscriber(self.gripper_ns + "joint_states", JointState, self.gripper_cb)
         self.MIN_FORCE = 0.05
-        self.MAX_FORCE = 50 # documentation says up to 70N is possible as continuous force
+        self.MAX_FORCE = 50  # documentation says up to 70N is possible as continuous force
         self.MIN_WIDTH = 0.0
         self.MAX_WIDTH = 0.08
         self.DEFAULT_EPSILON = 0.005
@@ -100,7 +101,7 @@ def grasp(self, width, speed=None, epsilon_outer=None, epsilon_inner=None, force
         goal.force = self.DEFAULT_FORCE if not force else force
         self.grasp_client.send_goal(goal)
         if wait_for_result:
-            result = self.grasp_client.wait_for_result(rospy.Duration(15.))
+            result = self.grasp_client.wait_for_result(rospy.Duration(10))
             return result
         return True
 
@@ -110,7 +111,7 @@ def move(self, width, speed=None, wait_for_result=False):
         goal.speed = self.DEFAULT_SPEED if not speed else speed
         self.move_client.send_goal(goal)
         if wait_for_result:
-            result = self.move_client.wait_for_result(rospy.Duration(15.))
+            result = self.move_client.wait_for_result(rospy.Duration(10))
             return result
         return True
 
@@ -118,7 +119,7 @@ def homing(self, wait_for_result=True):
         goal = HomingGoal()
         self.homing_client.send_goal(goal)
         if wait_for_result:
-            result = self.homing_client.wait_for_result(rospy.Duration(15.))
+            result = self.homing_client.wait_for_result(rospy.Duration(10))
             return result
         return True
 
@@ -126,6 +127,6 @@ def stop(self, wait_for_result=False):
         goal = StopGoal()
         self.stop_client.send_goal(goal)
         if wait_for_result:
-            result = self.stop_client.wait_for_result(rospy.Duration(15.))
+            result = self.stop_client.wait_for_result(rospy.Duration(10))
             return result
-        return True
\ No newline at end of file
+        return True

From 791e280eec7223ee477baf741b66f63c6a4ba13c Mon Sep 17 00:00:00 2001
From: Andrea <asipos@umich.edu>
Date: Tue, 22 Mar 2022 16:43:01 -0400
Subject: [PATCH 4/7] overwrote follow_arms_joint_trajectory with move_group
 execute

---
 .../scripts/tutorial/panda_basic_motion.py    | 28 +-----
 arm_robots/src/arm_robots/panda.py            | 88 ++++++++-----------
 arm_robots/src/arm_robots/robot.py            | 28 +++---
 3 files changed, 54 insertions(+), 90 deletions(-)

diff --git a/arm_robots/scripts/tutorial/panda_basic_motion.py b/arm_robots/scripts/tutorial/panda_basic_motion.py
index ab67936..dfb2e1f 100755
--- a/arm_robots/scripts/tutorial/panda_basic_motion.py
+++ b/arm_robots/scripts/tutorial/panda_basic_motion.py
@@ -7,27 +7,7 @@
     rospy.init_node('panda_motion')
     panda = Panda()
     panda.connect()
-    panda.set_execute(False)  # this will stop the robot from actually executing a path, good for testing
-    # panda.plan_to_position_cartesian(panda.nebula_arm, panda.nebula_wrist, target_position=[0.04, 0.2867, 1.17206])
-    # panda.plan_to_position_cartesian(panda.rocket_arm, panda.rocket_wrist, target_position=[-0.013, -0.04, 1.16967])
-    # poking_approach_pose_1 = [0.0, 0.0, 1.07, np.radians(135), np.radians(-90), np.radians(225)]
-    # poking_target_pose_1 = [-0.039, 0.0, 1.07, np.radians(135), np.radians(-90), np.radians(225)]
-    # poking_approach_pose_2 = [0.0, 0.015, 1.085, np.radians(135), np.radians(-90), np.radians(225)]
-    # poking_target_pose_2 = [-0.039, 0.015, 1.085, np.radians(135), np.radians(-90), np.radians(225)]
-    # poking_approach_pose_3 = [0.0, 0.015, 1.055, np.radians(135), np.radians(-90), np.radians(225)]
-    # poking_target_pose_3 = [-0.039, 0.015, 1.055, np.radians(135), np.radians(-90), np.radians(225)]
-    # poking_approach_pose_4 = [0.0, -0.015, 1.055, np.radians(135), np.radians(-90), np.radians(225)]
-    # poking_target_pose_4 = [-0.039, -0.015, 1.055, np.radians(135), np.radians(-90), np.radians(225)]
-    # poking_approach_pose_5 = [0.0, -0.015, 1.085, np.radians(135), np.radians(-90), np.radians(225)]
-    # poking_target_pose_5 = [-0.039, -0.015, 1.085, np.radians(135), np.radians(-90), np.radians(225)]
-    # pdb.set_trace()
-    # poking_approach_pose_1 = [0.29320, 0.00387, 0.75, 1.0, 0.0, 0.0, 0.0]
-    # poking_target_pose_1 = [0.29320, 0.00387, 0.723] #, 1.0, 0.0, 0.0, 0.0]
-    # time.sleep(1)
-    # panda.plan_to_pose(panda.rocket_arm, "rocket_EE", poking_approach_pose_1, "world")
-    # time.sleep(3)
-    # panda.plan_to_position_cartesian(panda.rocket_arm, "rocket_EE", poking_target_pose_1, 0.001)
-    # time.sleep(3)
-    # panda.plan_to_pose(panda.rocket_arm, "rocket_EE", poking_approach_pose_1, "world")
-    # time.sleep(3)
-    # TODO: Reference med_motion.py for examples.
+    # panda.set_execute(False)  # this will stop the robot from actually executing a path, good for testing
+    joint_config = [-0.007018571913499291, -1.3080290837538866, -0.0070561850128466625, -2.598759190173999, -0.008907794188422758, 1.295876371807555, 1.25]
+
+    panda.plan_to_joint_config('panda_2', joint_config)
diff --git a/arm_robots/src/arm_robots/panda.py b/arm_robots/src/arm_robots/panda.py
index 7e1d9ba..175b5f4 100755
--- a/arm_robots/src/arm_robots/panda.py
+++ b/arm_robots/src/arm_robots/panda.py
@@ -1,6 +1,11 @@
 #! /usr/bin/env python
+from typing import Optional, Callable, List, Tuple
+
 import actionlib
 import rospy
+from moveit_msgs.msg import RobotTrajectory
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
 from arm_robots.robot import MoveitEnabledRobot
 from franka_gripper.msg import GraspAction, GraspGoal, MoveAction, MoveGoal, HomingAction, HomingGoal, StopAction, \
     StopGoal
@@ -11,73 +16,45 @@
 
 class Panda(MoveitEnabledRobot):
 
-    def __init__(self, robot_namespace: str = 'combined_panda', force_trigger: float = -0.0, **kwargs):
+    def __init__(self, robot_namespace: str = '', force_trigger: float = -0.0, **kwargs):
         MoveitEnabledRobot.__init__(self,
                                     robot_namespace=robot_namespace,
                                     robot_description=ns_join(robot_namespace, 'robot_description'),
-                                    arms_controller_name='position_joint_trajectory_controller',
+                                    arms_controller_name=None,
                                     force_trigger=force_trigger,
                                     **kwargs)
-        self.nebula_arm = "nebula_arm"
-        self.nebula_id = "panda_1"
-        self.rocket_arm = "rocket_arm"
-        self.rocket_id = "panda_2"
-        self.nebula_wrist = "panda_1_link8"
-        self.rocket_wrist = "panda_2_link8"
-        self.nebula_gripper = PandaGripper(self.robot_namespace, self.nebula_id)
-        self.rocket_gripper = PandaGripper(self.robot_namespace, self.rocket_id)
-        rospy.Subscriber(ns_join(self.robot_namespace, f'{self.nebula_id}_state_controller/franka_states'), FrankaState,
-                         self.nebula_error_cb)
-        rospy.Subscriber(ns_join(self.robot_namespace, f'{self.rocket_id}_state_controller/franka_states'), FrankaState,
-                         self.rocket_error_cb)
-        self.error_client = actionlib.SimpleActionClient(ns_join(self.robot_namespace, "error_recovery"),
-                                                         ErrorRecoveryAction)
-        self.error_client.wait_for_server()
-
-    def open_nebula_gripper(self):
-        self.nebula_gripper.move(self.nebula_gripper.MAX_WIDTH)
-
-    def close_nebula_gripper(self):
-        self.nebula_gripper.move(self.nebula_gripper.MIN_WIDTH)
-
-    def open_rocket_gripper(self):
-        self.rocket_gripper.move(self.rocket_gripper.MAX_WIDTH)
-
-    def close_rocket_gripper(self):
-        self.rocket_gripper.move(self.rocket_gripper.MIN_WIDTH)
-
-    def nebula_error_cb(self, data):
-        if data.current_errors.communication_constraints_violation or data.last_motion_errors.communication_constraints_violation:
-            self.clear_errors(wait_for_result=True)
-
-    def rocket_error_cb(self, data):
-        if data.current_errors.communication_constraints_violation or data.last_motion_errors.communication_constraints_violation:
-            self.clear_errors(wait_for_result=True)
-
-    def clear_errors(self, wait_for_result=False):
-        goal = ErrorRecoveryGoal()
-        self.error_client.send_goal(goal)
-        if wait_for_result:
-            result = self.error_client.wait_for_result(rospy.Duration(10))
-            return result
-        return True
-
+        self.panda_1 = 'panda_1'
+        self.panda_2 = 'panda_2'
+        self.display_goals = False
+        # self.panda_1.gripper = PandaGripper(self.robot_namespace, self.panda_1)
+        # self.panda_2.gripper = PandaGripper(self.robot_namespace, self.panda_2)
+
+    def follow_arms_joint_trajectory(self, trajectory: JointTrajectory, stop_condition: Optional[Callable] = None,
+                                     group_name: Optional[str] = None):
+        move_group = self.get_move_group_commander(group_name)
+        plan_msg = RobotTrajectory()
+        plan_msg.joint_trajectory = trajectory
+        move_group.execute(plan_msg)
+        pass
+
+    def send_joint_command(self, joint_names: List[str], trajectory_point: JointTrajectoryPoint) -> Tuple[bool, str]:
+        pass
     # TODO: Add control mode setter/getter.
 
 
 class PandaGripper:
     def __init__(self, robot_ns, arm_id):
-        self.gripper_ns = "/" + robot_ns + "/" + arm_id + "/franka_gripper/"
-        self.grasp_client = actionlib.SimpleActionClient(self.gripper_ns + "grasp", GraspAction)
-        self.move_client = actionlib.SimpleActionClient(self.gripper_ns + "move", MoveAction)
-        self.homing_client = actionlib.SimpleActionClient(self.gripper_ns + "homing", HomingAction)
-        self.stop_client = actionlib.SimpleActionClient(self.gripper_ns + "stop", StopAction)
+        self.gripper_ns = ns_join(robot_ns, f'{arm_id}/franka_gripper')
+        self.grasp_client = actionlib.SimpleActionClient(ns_join(self.gripper_ns, 'grasp'), GraspAction)
+        self.move_client = actionlib.SimpleActionClient(ns_join(self.gripper_ns, 'move'), MoveAction)
+        self.homing_client = actionlib.SimpleActionClient(ns_join(self.gripper_ns, 'homing'), HomingAction)
+        self.stop_client = actionlib.SimpleActionClient(ns_join(self.gripper_ns, 'stop'), StopAction)
         self.grasp_client.wait_for_server()
         self.move_client.wait_for_server()
         self.homing_client.wait_for_server()
         self.stop_client.wait_for_server()
         self.gripper_width = None
-        rospy.Subscriber(self.gripper_ns + "joint_states", JointState, self.gripper_cb)
+        rospy.Subscriber(ns_join(self.gripper_ns, 'joint_states'), JointState, self.gripper_cb)
         self.MIN_FORCE = 0.05
         self.MAX_FORCE = 50  # documentation says up to 70N is possible as continuous force
         self.MIN_WIDTH = 0.0
@@ -85,7 +62,6 @@ def __init__(self, robot_ns, arm_id):
         self.DEFAULT_EPSILON = 0.005
         self.DEFAULT_SPEED = 0.02
         self.DEFAULT_FORCE = 10
-        # TODO: add to cfg file
 
     def gripper_cb(self, data):
         self.gripper_width = data.position[0] + data.position[1]
@@ -130,3 +106,9 @@ def stop(self, wait_for_result=False):
             result = self.stop_client.wait_for_result(rospy.Duration(10))
             return result
         return True
+
+    def open(self):
+        self.move(self.MAX_WIDTH)
+
+    def close(self):
+        self.move(self.MIN_WIDTH)
diff --git a/arm_robots/src/arm_robots/robot.py b/arm_robots/src/arm_robots/robot.py
index 84daaf6..133cd1c 100644
--- a/arm_robots/src/arm_robots/robot.py
+++ b/arm_robots/src/arm_robots/robot.py
@@ -105,14 +105,16 @@ def connect(self, preload_move_groups=True):
                 self.get_move_group_commander(group_name)
 
     def setup_joint_trajectory_controller_client(self, controller_name):
-        action_name = ns_join(self.robot_namespace, ns_join(controller_name, "follow_joint_trajectory"))
-        client = SimpleActionClient(action_name, FollowJointTrajectoryAction)
-        resolved_action_name = rospy.resolve_name(action_name)
-        wait_msg = f"Waiting for joint trajectory follower server {resolved_action_name}..."
-        rospy.loginfo(wait_msg)
-        client.wait_for_server()
-        rospy.loginfo(f"Connected.")
-        return client
+        if controller_name is not None:
+            action_name = ns_join(self.robot_namespace, ns_join(controller_name, "follow_joint_trajectory"))
+            client = SimpleActionClient(action_name, FollowJointTrajectoryAction)
+            resolved_action_name = rospy.resolve_name(action_name)
+            wait_msg = f"Waiting for joint trajectory follower server {resolved_action_name}..."
+            rospy.loginfo(wait_msg)
+            client.wait_for_server()
+            rospy.loginfo(f"Connected.")
+            return client
+        return None
 
     def set_execute(self, execute: bool):
         self.execute = execute
@@ -148,7 +150,7 @@ def plan_to_position(self,
         if self.raise_on_failure and not planning_result.success:
             raise RobotPlanningError(f"Plan to position failed {planning_result.planning_error_code}")
 
-        execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition)
+        execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, group_name)
         return PlanningAndExecutionResult(planning_result, execution_result)
 
     def plan_to_position_cartesian(self,
@@ -176,7 +178,7 @@ def plan_to_position_cartesian(self,
         if self.raise_on_failure and not planning_result.success:
             raise RobotPlanningError(f"Cartesian path is only {fraction * 100}% complete")
 
-        execution_result = self.follow_arms_joint_trajectory(plan.joint_trajectory, stop_condition)
+        execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, group_name)
         return PlanningAndExecutionResult(planning_result, execution_result)
 
     def plan_to_pose(self, group_name, ee_link_name, target_pose, frame_id: str = 'robot_root',
@@ -197,7 +199,7 @@ def plan_to_pose(self, group_name, ee_link_name, target_pose, frame_id: str = 'r
         if self.raise_on_failure and not planning_result.success:
             raise RobotPlanningError(f"Plan to pose failed {planning_result.planning_error_code}")
 
-        execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition)
+        execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, group_name)
         return PlanningAndExecutionResult(planning_result, execution_result)
 
     def get_link_pose(self, link_name: str):
@@ -243,7 +245,7 @@ def plan_to_joint_config(self, group_name: str, joint_config: Union[List, Dict,
         planning_result = PlanningResult(move_group.plan())
         if self.raise_on_failure and not planning_result.success:
             raise RobotPlanningError(f"Plan to position failed {planning_result.planning_error_code}")
-        execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition)
+        execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, group_name)
         return PlanningAndExecutionResult(planning_result, execution_result)
 
     def make_follow_joint_trajectory_goal(self, trajectory) -> FollowJointTrajectoryGoal:
@@ -297,7 +299,7 @@ def follow_joint_config(self, joint_names: List[str], joint_positions, client: S
         trajectory.points.append(point)
         return self.follow_joint_trajectory(trajectory, client)
 
-    def follow_arms_joint_trajectory(self, trajectory: JointTrajectory, stop_condition: Optional[Callable] = None):
+    def follow_arms_joint_trajectory(self, trajectory: JointTrajectory, stop_condition: Optional[Callable] = None, group_name: Optional[str] = None):
         return self.follow_joint_trajectory(trajectory, self.arms_client, stop_condition=stop_condition)
 
     def distance(self, ee_link_name: str, target_position):

From f611b2924fd37476a005f1e7f9ddc5fc750216f5 Mon Sep 17 00:00:00 2001
From: Andrea <asipos@umich.edu>
Date: Thu, 31 Mar 2022 14:51:42 -0400
Subject: [PATCH 5/7] updated gripper commands and fixed planning groups

---
 .../scripts/tutorial/panda_basic_motion.py     |  9 ++++++---
 arm_robots/src/arm_robots/panda.py             | 18 +++++++++++++-----
 2 files changed, 19 insertions(+), 8 deletions(-)

diff --git a/arm_robots/scripts/tutorial/panda_basic_motion.py b/arm_robots/scripts/tutorial/panda_basic_motion.py
index dfb2e1f..7d2e422 100755
--- a/arm_robots/scripts/tutorial/panda_basic_motion.py
+++ b/arm_robots/scripts/tutorial/panda_basic_motion.py
@@ -1,4 +1,5 @@
 #! /usr/bin/env python
+import pdb
 
 import rospy
 from arm_robots.panda import Panda
@@ -7,7 +8,9 @@
     rospy.init_node('panda_motion')
     panda = Panda()
     panda.connect()
-    # panda.set_execute(False)  # this will stop the robot from actually executing a path, good for testing
-    joint_config = [-0.007018571913499291, -1.3080290837538866, -0.0070561850128466625, -2.598759190173999, -0.008907794188422758, 1.295876371807555, 1.25]
+    panda.set_execute(False)  # this will stop the robot from actually executing a path, good for testing
+    panda.plan_to_pose(panda.panda_1, panda.panda_1_EE, [0.2, 0.25, 1.2, 0.0, 3.14, 1.57], frame_id="world")
 
-    panda.plan_to_joint_config('panda_2', joint_config)
+    # joint_config = [-0.007018571913499291, -1.3080290837538866, -0.0070561850128466625, -2.598759190173999, -0.008907794188422758, 1.295876371807555, 1.25]
+    #
+    # panda.plan_to_joint_config('panda_2', joint_config)
diff --git a/arm_robots/src/arm_robots/panda.py b/arm_robots/src/arm_robots/panda.py
index 175b5f4..7e83f06 100755
--- a/arm_robots/src/arm_robots/panda.py
+++ b/arm_robots/src/arm_robots/panda.py
@@ -1,4 +1,6 @@
 #! /usr/bin/env python
+import pdb
+import rosnode
 from typing import Optional, Callable, List, Tuple
 
 import actionlib
@@ -25,9 +27,15 @@ def __init__(self, robot_namespace: str = '', force_trigger: float = -0.0, **kwa
                                     **kwargs)
         self.panda_1 = 'panda_1'
         self.panda_2 = 'panda_2'
+        self.panda_1_EE = 'panda_1_link_planning_EE'
+        self.panda_2_EE = 'panda_2_link_planning_EE'
         self.display_goals = False
-        # self.panda_1.gripper = PandaGripper(self.robot_namespace, self.panda_1)
-        # self.panda_2.gripper = PandaGripper(self.robot_namespace, self.panda_2)
+
+        active_nodes = rosnode.get_node_names()
+        if ns_join('/', f'{self.panda_1}/franka_gripper') in active_nodes:
+            self.gripper_1 = PandaGripper(self.robot_namespace, self.panda_1)
+        if ns_join('/', f'{self.panda_2}/franka_gripper') in active_nodes:
+            self.gripper_2 = PandaGripper(self.robot_namespace, self.panda_2)
 
     def follow_arms_joint_trajectory(self, trajectory: JointTrajectory, stop_condition: Optional[Callable] = None,
                                      group_name: Optional[str] = None):
@@ -66,7 +74,7 @@ def __init__(self, robot_ns, arm_id):
     def gripper_cb(self, data):
         self.gripper_width = data.position[0] + data.position[1]
 
-    def grasp(self, width, speed=None, epsilon_outer=None, epsilon_inner=None, force=None, wait_for_result=False):
+    def grasp(self, width, speed=None, epsilon_outer=None, epsilon_inner=None, force=None, wait_for_result=True):
         if width > self.gripper_width:
             self.move(self.MAX_WIDTH, wait_for_result=True)
         goal = GraspGoal()
@@ -81,7 +89,7 @@ def grasp(self, width, speed=None, epsilon_outer=None, epsilon_inner=None, force
             return result
         return True
 
-    def move(self, width, speed=None, wait_for_result=False):
+    def move(self, width, speed=None, wait_for_result=True):
         goal = MoveGoal()
         goal.width = width
         goal.speed = self.DEFAULT_SPEED if not speed else speed
@@ -99,7 +107,7 @@ def homing(self, wait_for_result=True):
             return result
         return True
 
-    def stop(self, wait_for_result=False):
+    def stop(self, wait_for_result=True):
         goal = StopGoal()
         self.stop_client.send_goal(goal)
         if wait_for_result:

From 56cb555c62915f17facfd50dcb04712a3b9cf9ed Mon Sep 17 00:00:00 2001
From: Andrea <asipos@umich.edu>
Date: Fri, 1 Apr 2022 14:35:25 -0400
Subject: [PATCH 6/7] added fake execution flag

---
 arm_robots/scripts/tutorial/panda_basic_motion.py | 13 ++++++++++++-
 arm_robots/src/arm_robots/panda.py                | 10 +++++-----
 2 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/arm_robots/scripts/tutorial/panda_basic_motion.py b/arm_robots/scripts/tutorial/panda_basic_motion.py
index 7d2e422..5714056 100755
--- a/arm_robots/scripts/tutorial/panda_basic_motion.py
+++ b/arm_robots/scripts/tutorial/panda_basic_motion.py
@@ -2,15 +2,26 @@
 import pdb
 
 import rospy
+import numpy as np
 from arm_robots.panda import Panda
 
 if __name__ == '__main__':
     rospy.init_node('panda_motion')
     panda = Panda()
     panda.connect()
+    print("Panda connected! Planning now...")
+    # pdb.set_trace()
     panda.set_execute(False)  # this will stop the robot from actually executing a path, good for testing
-    panda.plan_to_pose(panda.panda_1, panda.panda_1_EE, [0.2, 0.25, 1.2, 0.0, 3.14, 1.57], frame_id="world")
+    if not panda.execute:
+        print("Real execution disabled.")
+    # pose_1_start = [0.07, 0.24, 1.3, -0.7071068, 0.7071068, 0, 0]
+    # pose_2_start = [0.07, 0.24, 1.1, np.radians(-90), np.radians(-90), 0]
+    # panda.plan_to_pose(panda.panda_1, panda.panda_1_EE, pose_1_start, frame_id="world")
+    # panda.plan_to_pose(panda.panda_2, panda.panda_2_EE, pose_2_start, frame_id="world")
 
+    pose_1_end = [0.07, 0.24, 1.2, -0.7071068, 0.7071068, 0, 0]
+    panda.plan_to_position_cartesian(panda.panda_1, panda.panda_1_EE, pose_1_end[:3])
+    pdb.set_trace()
     # joint_config = [-0.007018571913499291, -1.3080290837538866, -0.0070561850128466625, -2.598759190173999, -0.008907794188422758, 1.295876371807555, 1.25]
     #
     # panda.plan_to_joint_config('panda_2', joint_config)
diff --git a/arm_robots/src/arm_robots/panda.py b/arm_robots/src/arm_robots/panda.py
index 7e83f06..df71d0e 100755
--- a/arm_robots/src/arm_robots/panda.py
+++ b/arm_robots/src/arm_robots/panda.py
@@ -39,11 +39,11 @@ def __init__(self, robot_namespace: str = '', force_trigger: float = -0.0, **kwa
 
     def follow_arms_joint_trajectory(self, trajectory: JointTrajectory, stop_condition: Optional[Callable] = None,
                                      group_name: Optional[str] = None):
-        move_group = self.get_move_group_commander(group_name)
-        plan_msg = RobotTrajectory()
-        plan_msg.joint_trajectory = trajectory
-        move_group.execute(plan_msg)
-        pass
+        if self.execute:
+            move_group = self.get_move_group_commander(group_name)
+            plan_msg = RobotTrajectory()
+            plan_msg.joint_trajectory = trajectory
+            move_group.execute(plan_msg)
 
     def send_joint_command(self, joint_names: List[str], trajectory_point: JointTrajectoryPoint) -> Tuple[bool, str]:
         pass

From ec1ec8a611eec5dd4276458160423bfec02ec4a8 Mon Sep 17 00:00:00 2001
From: Andrea <asipos@umich.edu>
Date: Mon, 4 Dec 2023 15:47:54 -0500
Subject: [PATCH 7/7] push andrea's stack to pandas branch

---
 .../scripts/tutorial/panda_basic_motion.py    |  52 +++++-
 arm_robots/src/arm_robots/panda.py            |  69 +++++--
 arm_robots/src/arm_robots/robot.py            | 170 +++---------------
 3 files changed, 121 insertions(+), 170 deletions(-)

diff --git a/arm_robots/scripts/tutorial/panda_basic_motion.py b/arm_robots/scripts/tutorial/panda_basic_motion.py
index 5714056..6778b84 100755
--- a/arm_robots/scripts/tutorial/panda_basic_motion.py
+++ b/arm_robots/scripts/tutorial/panda_basic_motion.py
@@ -4,6 +4,16 @@
 import rospy
 import numpy as np
 from arm_robots.panda import Panda
+from moveit_msgs.msg import RobotTrajectory
+import tf
+
+
+def execute_plan(panda, group_name, joint_trajectory):
+    move_group = panda.get_move_group_commander(group_name)
+    plan_msg = RobotTrajectory()
+    plan_msg.joint_trajectory = joint_trajectory
+    move_group.execute(plan_msg)
+
 
 if __name__ == '__main__':
     rospy.init_node('panda_motion')
@@ -14,14 +24,40 @@
     panda.set_execute(False)  # this will stop the robot from actually executing a path, good for testing
     if not panda.execute:
         print("Real execution disabled.")
-    # pose_1_start = [0.07, 0.24, 1.3, -0.7071068, 0.7071068, 0, 0]
-    # pose_2_start = [0.07, 0.24, 1.1, np.radians(-90), np.radians(-90), 0]
-    # panda.plan_to_pose(panda.panda_1, panda.panda_1_EE, pose_1_start, frame_id="world")
-    # panda.plan_to_pose(panda.panda_2, panda.panda_2_EE, pose_2_start, frame_id="world")
+    panda_1_action_1_start = [0.25, 0.25, 1.16, np.radians(180), 0, np.radians(-90)]
+    panda_2_action_1_start = [0.25, 0.15, 1.16, np.radians(-90), np.radians(-90), 0]
+    panda_1_action_1_start_plan = panda.plan_to_pose(panda.panda_1, panda.panda_1_EE, panda_1_action_1_start, frame_id="world")
+    panda_2_action_1_start_plan = panda.plan_to_pose(panda.panda_2, panda.panda_2_EE, panda_2_action_1_start, frame_id="world")
+
+    pdb.set_trace()
+
+    execute_plan(panda, panda.panda_1, panda_1_action_1_start_plan.planning_result.plan.joint_trajectory)
+    execute_plan(panda, panda.panda_2, panda_2_action_1_start_plan.planning_result.plan.joint_trajectory)
+    panda.set_execute(False)
+
+    pdb.set_trace()
+
+    panda_1_action_2_start = [0.25, 0.25, 1.4, np.radians(90), np.radians(-90), 0]
+    panda_2_action_2_start = [0.25, 0.15, 1.36, np.radians(-90), np.radians(-90), 0]
+    panda_1_action_2_start_plan = panda.plan_to_pose(panda.panda_1, panda.panda_1_EE, panda_1_action_2_start, frame_id="world")
+    panda_2_action_2_start_plan = panda.plan_to_pose(panda.panda_2, panda.panda_2_EE, panda_2_action_2_start, frame_id="world")
 
-    pose_1_end = [0.07, 0.24, 1.2, -0.7071068, 0.7071068, 0, 0]
-    panda.plan_to_position_cartesian(panda.panda_1, panda.panda_1_EE, pose_1_end[:3])
     pdb.set_trace()
-    # joint_config = [-0.007018571913499291, -1.3080290837538866, -0.0070561850128466625, -2.598759190173999, -0.008907794188422758, 1.295876371807555, 1.25]
+
+    execute_plan(panda, panda.panda_1, panda_1_action_2_start_plan.planning_result.plan.joint_trajectory)
+    execute_plan(panda, panda.panda_2, panda_2_action_2_start_plan.planning_result.plan.joint_trajectory)
+    panda.set_execute(False)
+
+    pdb.set_trace()
+
+    # panda_2_action_1_end = [0.25, 0.20, 1.155, np.radians(-90), np.radians(-90), 0]
+    # pt = [[np.array(panda_2_action_1_end[:3])]]
+    # ori = [tf.transformations.quaternion_from_euler(panda_2_action_1_end[3], panda_2_action_1_end[4], panda_2_action_1_end[5])]
+    # panda_2_action_1_end_plan = panda.follow_jacobian_to_position(panda.panda_2, ['panda_2_link_planning_EE'], pt, ori, 0.01)
     #
-    # panda.plan_to_joint_config('panda_2', joint_config)
+    # pdb.set_trace()
+    #
+    # execute_plan(panda, panda.panda_2, panda_2_action_1_end_plan.planning_result.plan.joint_trajectory)
+    #
+    # pdb.set_trace()
+
diff --git a/arm_robots/src/arm_robots/panda.py b/arm_robots/src/arm_robots/panda.py
index df71d0e..7fc7e75 100755
--- a/arm_robots/src/arm_robots/panda.py
+++ b/arm_robots/src/arm_robots/panda.py
@@ -1,19 +1,23 @@
 #! /usr/bin/env python
 import pdb
-import rosnode
 from typing import Optional, Callable, List, Tuple
 
 import actionlib
 import rospy
+import numpy as np
+from geometry_msgs.msg import WrenchStamped
 from moveit_msgs.msg import RobotTrajectory
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 
 from arm_robots.robot import MoveitEnabledRobot
 from franka_gripper.msg import GraspAction, GraspGoal, MoveAction, MoveGoal, HomingAction, HomingGoal, StopAction, \
     StopGoal
+from netft_rdt_driver.srv import Zero
+from control_msgs.msg import GripperCommandActionGoal, GripperCommand, GripperCommandGoal
 from franka_msgs.msg import ErrorRecoveryAction, ErrorRecoveryGoal, FrankaState
 from rosgraph.names import ns_join
 from sensor_msgs.msg import JointState
+from arm_robots.robot_utils import ExecutionResult
 
 
 class Panda(MoveitEnabledRobot):
@@ -27,27 +31,54 @@ def __init__(self, robot_namespace: str = '', force_trigger: float = -0.0, **kwa
                                     **kwargs)
         self.panda_1 = 'panda_1'
         self.panda_2 = 'panda_2'
-        self.panda_1_EE = 'panda_1_link_planning_EE'
-        self.panda_2_EE = 'panda_2_link_planning_EE'
+        self.panda_1_command_pub = rospy.Publisher(ns_join(self.panda_1, 'panda_1_position_joint_trajectory_controller/command'), JointTrajectory, queue_size=10)
+        self.panda_2_command_pub = rospy.Publisher(ns_join(self.panda_2, 'panda_2_position_joint_trajectory_controller/command'), JointTrajectory, queue_size=10)
         self.display_goals = False
+        self.panda_1_gripper = PandaGripper(self.robot_namespace, self.panda_1)
+        self.panda_2_gripper = PandaGripper(self.robot_namespace, self.panda_2)
+        self.panda_1_netft = PandaNetft(self.robot_namespace, self.panda_1)
+        self.panda_2_netft = PandaNetft(self.robot_namespace, self.panda_2)
 
-        active_nodes = rosnode.get_node_names()
-        if ns_join('/', f'{self.panda_1}/franka_gripper') in active_nodes:
-            self.gripper_1 = PandaGripper(self.robot_namespace, self.panda_1)
-        if ns_join('/', f'{self.panda_2}/franka_gripper') in active_nodes:
-            self.gripper_2 = PandaGripper(self.robot_namespace, self.panda_2)
+        self.panda_1_client = self.setup_joint_trajectory_controller_client(ns_join(self.panda_1, f'{self.panda_1}_position_joint_trajectory_controller'))
+        self.panda_2_client = self.setup_joint_trajectory_controller_client(ns_join(self.panda_2, f'{self.panda_2}_position_joint_trajectory_controller'))
 
     def follow_arms_joint_trajectory(self, trajectory: JointTrajectory, stop_condition: Optional[Callable] = None,
                                      group_name: Optional[str] = None):
-        if self.execute:
-            move_group = self.get_move_group_commander(group_name)
-            plan_msg = RobotTrajectory()
-            plan_msg.joint_trajectory = trajectory
-            move_group.execute(plan_msg)
+        if group_name == self.panda_1:
+            return self.follow_joint_trajectory(trajectory, self.panda_1_client, stop_condition=stop_condition)
+        elif group_name == self.panda_2:
+            return self.follow_joint_trajectory(trajectory, self.panda_2_client, stop_condition=stop_condition)
 
-    def send_joint_command(self, joint_names: List[str], trajectory_point: JointTrajectoryPoint) -> Tuple[bool, str]:
-        pass
-    # TODO: Add control mode setter/getter.
+    def send_joint_command(self, joint_names: List[str], trajectory_point: JointTrajectoryPoint, group_name: Optional[str] = None):
+        robot_command = JointTrajectory()
+        robot_command.joint_names = joint_names
+        robot_command.points.append(trajectory_point)
+        pdb.set_trace()
+        if group_name == self.panda_1:
+            self.panda_1_command_pub.publish(robot_command)
+        elif group_name == self.panda_2:
+            self.panda_2_command_pub.publish(robot_command)
+
+
+
+class PandaNetft:
+    def __init__(self, robot_ns, arm_id, stop_force=3.0, stop_torque=0.3):
+        self.netft_ns = ns_join(robot_ns, f'{arm_id}_netft')
+        self.netft_zero = rospy.ServiceProxy(ns_join(self.netft_ns, 'zero'), Zero)
+        self.netft_data = None
+        self.netft_data_sub = rospy.Subscriber(ns_join(self.netft_ns, 'netft_data'), WrenchStamped, self.netft_data_cb, queue_size=None)
+        self.stop_force = stop_force
+        self.stop_torque = stop_torque
+
+    def zero_netft(self):
+        self.netft_zero()
+
+    def netft_data_cb(self, wrench_msg):
+        self.netft_data = np.array((wrench_msg.wrench.force.x, wrench_msg.wrench.force.y, wrench_msg.wrench.force.z, wrench_msg.wrench.torque.x, wrench_msg.wrench.torque.y, wrench_msg.wrench.torque.z))
+
+    def stop_condition(self, feedback):
+        force_magnitude = np.linalg.norm(self.netft_data[:3])
+        return force_magnitude > self.stop_force
 
 
 class PandaGripper:
@@ -74,7 +105,7 @@ def __init__(self, robot_ns, arm_id):
     def gripper_cb(self, data):
         self.gripper_width = data.position[0] + data.position[1]
 
-    def grasp(self, width, speed=None, epsilon_outer=None, epsilon_inner=None, force=None, wait_for_result=True):
+    def grasp(self, width, speed=None, epsilon_outer=None, epsilon_inner=None, force=None, wait_for_result=False):
         if width > self.gripper_width:
             self.move(self.MAX_WIDTH, wait_for_result=True)
         goal = GraspGoal()
@@ -89,7 +120,7 @@ def grasp(self, width, speed=None, epsilon_outer=None, epsilon_inner=None, force
             return result
         return True
 
-    def move(self, width, speed=None, wait_for_result=True):
+    def move(self, width, speed=None, wait_for_result=False):
         goal = MoveGoal()
         goal.width = width
         goal.speed = self.DEFAULT_SPEED if not speed else speed
@@ -107,7 +138,7 @@ def homing(self, wait_for_result=True):
             return result
         return True
 
-    def stop(self, wait_for_result=True):
+    def stop(self, wait_for_result=False):
         goal = StopGoal()
         self.stop_client.send_goal(goal)
         if wait_for_result:
diff --git a/arm_robots/src/arm_robots/robot.py b/arm_robots/src/arm_robots/robot.py
index 133cd1c..e83bd9f 100644
--- a/arm_robots/src/arm_robots/robot.py
+++ b/arm_robots/src/arm_robots/robot.py
@@ -1,8 +1,8 @@
 #! /usr/bin/env python
+import pdb
 from typing import List, Union, Tuple, Callable, Optional, Dict
 
 import numpy as np
-import pyjacobian_follower
 from matplotlib import colors
 
 import moveit_commander
@@ -55,17 +55,16 @@ def __init__(self,
                  block: bool = True,
                  raise_on_failure: bool = False,
                  display_goals: bool = True,
-                 force_trigger: float = 9.0,
-                 jacobian_follower: Optional[pyjacobian_follower.JacobianFollower] = None):
+                 force_trigger: float = 9.0):
         super().__init__(robot_namespace, robot_description)
         self._max_velocity_scale_factor = 0.1
+        self._max_acceleration_scale_factor = 0.1
         self.stored_tool_orientations = None
         self.raise_on_failure = raise_on_failure
         self.execute = execute
         self.block = block
         self.display_goals = display_goals
         self.force_trigger = force_trigger
-        self.jacobian_follower = jacobian_follower
 
         self.arms_controller_name = arms_controller_name
 
@@ -73,17 +72,10 @@ def __init__(self,
         self.display_goal_position_pub = rospy.Publisher('goal_position', Marker, queue_size=10)
         self.display_robot_state_pubs = {}
         self.display_robot_traj_pubs = {}
+        self.scene = None
 
         self.arms_client = None
 
-        if jacobian_follower is None:
-            self.jacobian_follower = pyjacobian_follower.JacobianFollower(robot_namespace=self.robot_namespace,
-                                                                          robot_description=robot_description,
-                                                                          translation_step_size=0.005,
-                                                                          minimize_rotation=True,
-                                                                          collision_check=True,
-                                                                          visualize=True)
-
         self.feedback_callbacks = []
         self._move_groups = {}
 
@@ -96,14 +88,14 @@ def connect(self, preload_move_groups=True):
         """
         super().connect()
 
-        self.jacobian_follower.connect_to_psm()
-
         # TODO: bad api? raii? this class isn't fully usable by the time it's constructor finishes, that's bad.
         self.arms_client = self.setup_joint_trajectory_controller_client(self.arms_controller_name)
         if preload_move_groups:
             for group_name in self.robot_commander.get_group_names():
                 self.get_move_group_commander(group_name)
 
+        self.scene = moveit_commander.PlanningSceneInterface()
+
     def setup_joint_trajectory_controller_client(self, controller_name):
         if controller_name is not None:
             action_name = ns_join(self.robot_namespace, ns_join(controller_name, "follow_joint_trajectory"))
@@ -135,6 +127,7 @@ def get_move_group_commander(self, group_name: str) -> moveit_commander.MoveGrou
         # faster than the kuka controller's max velocity, otherwise the kuka controllers will error out.
         safety_factor = 0.9
         move_group.set_max_velocity_scaling_factor(self._max_velocity_scale_factor * safety_factor)
+        move_group.set_max_acceleration_scaling_factor(self._max_acceleration_scale_factor * safety_factor)
         return move_group
 
     def plan_to_position(self,
@@ -150,7 +143,8 @@ def plan_to_position(self,
         if self.raise_on_failure and not planning_result.success:
             raise RobotPlanningError(f"Plan to position failed {planning_result.planning_error_code}")
 
-        execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, group_name)
+        execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition,
+                                                             group_name)
         return PlanningAndExecutionResult(planning_result, execution_result)
 
     def plan_to_position_cartesian(self,
@@ -178,19 +172,22 @@ def plan_to_position_cartesian(self,
         if self.raise_on_failure and not planning_result.success:
             raise RobotPlanningError(f"Cartesian path is only {fraction * 100}% complete")
 
-        execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, group_name)
+        execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition,
+                                                             group_name)
         return PlanningAndExecutionResult(planning_result, execution_result)
 
     def plan_to_pose(self, group_name, ee_link_name, target_pose, frame_id: str = 'robot_root',
-                     stop_condition: Optional[Callable] = None):
+                     stop_condition: Optional[Callable] = None, position_tol: float = 0.0005,
+                     orientation_tol: float = 0.005, timeout: float = 5.0):
         self.check_inputs(group_name, ee_link_name)
         move_group = self.get_move_group_commander(group_name)
+        move_group.set_planning_time(timeout)
         move_group.set_end_effector_link(ee_link_name)
         target_pose_stamped = convert_to_pose_msg(target_pose)
         target_pose_stamped.header.frame_id = frame_id
         move_group.set_pose_target(target_pose_stamped)
-        move_group.set_goal_position_tolerance(0.002)
-        move_group.set_goal_orientation_tolerance(0.02)
+        move_group.set_goal_position_tolerance(position_tol)
+        move_group.set_goal_orientation_tolerance(orientation_tol)
 
         if self.display_goals:
             self.display_goal_pose(target_pose_stamped.pose, frame_id, ee_link_name)
@@ -198,8 +195,8 @@ def plan_to_pose(self, group_name, ee_link_name, target_pose, frame_id: str = 'r
         planning_result = PlanningResult(move_group.plan())
         if self.raise_on_failure and not planning_result.success:
             raise RobotPlanningError(f"Plan to pose failed {planning_result.planning_error_code}")
-
-        execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, group_name)
+        execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition,
+                                                             group_name)
         return PlanningAndExecutionResult(planning_result, execution_result)
 
     def get_link_pose(self, link_name: str):
@@ -236,6 +233,8 @@ def plan_to_joint_config(self, group_name: str, joint_config: Union[List, Dict,
             joint_config = {name: val for name, val in zip(move_group.get_active_joints(), joint_config)}
 
         move_group.set_joint_value_target(joint_config)
+        # move_group.set_goal_position_tolerance(0.0005)
+        # move_group.set_goal_orientation_tolerance(0.005)
 
         if self.display_goals:
             robot_state = RobotState(joint_state=JointState(name=list(joint_config.keys()),
@@ -245,7 +244,8 @@ def plan_to_joint_config(self, group_name: str, joint_config: Union[List, Dict,
         planning_result = PlanningResult(move_group.plan())
         if self.raise_on_failure and not planning_result.success:
             raise RobotPlanningError(f"Plan to position failed {planning_result.planning_error_code}")
-        execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, group_name)
+        execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition,
+                                                             group_name)
         return PlanningAndExecutionResult(planning_result, execution_result)
 
     def make_follow_joint_trajectory_goal(self, trajectory) -> FollowJointTrajectoryGoal:
@@ -277,6 +277,9 @@ def _feedback_cb(feedback: FollowJointTrajectoryFeedback):
                     feedback_callback(client, goal, feedback)
                 if stop_condition is not None and stop_condition(feedback):
                     client.cancel_all_goals()
+                    info_msg = "Stop condition met! Canceled all goals."
+                    rospy.loginfo(info_msg)
+
 
             # NOTE: this is where execution is actually requested in the form of a joint trajectory
             client.send_goal(goal, feedback_cb=_feedback_cb)
@@ -299,7 +302,8 @@ def follow_joint_config(self, joint_names: List[str], joint_positions, client: S
         trajectory.points.append(point)
         return self.follow_joint_trajectory(trajectory, client)
 
-    def follow_arms_joint_trajectory(self, trajectory: JointTrajectory, stop_condition: Optional[Callable] = None, group_name: Optional[str] = None):
+    def follow_arms_joint_trajectory(self, trajectory: JointTrajectory, stop_condition: Optional[Callable] = None,
+                                     group_name: Optional[str] = None):
         return self.follow_joint_trajectory(trajectory, self.arms_client, stop_condition=stop_condition)
 
     def distance(self, ee_link_name: str, target_position):
@@ -307,126 +311,6 @@ def distance(self, ee_link_name: str, target_position):
         error = np.linalg.norm(ros_numpy.numpify(current_pose.position) - target_position)
         return error
 
-    def get_orientation(self, name: str):
-        link = self.robot_commander.get_link(name)
-        return ros_numpy.numpify(link.pose().pose.orientation)
-
-    def store_tool_orientations(self, preferred_tool_orientations: Optional[Dict]):
-        """ dict values can be Pose, Quaternion, or just a numpy array/list """
-        self.stored_tool_orientations = {}
-        for k, v in preferred_tool_orientations.items():
-            if isinstance(v, Pose):
-                q = ros_numpy.numpify(v.orientation)
-                self.stored_tool_orientations[k] = q
-            elif isinstance(v, Quaternion):
-                q = ros_numpy.numpify(v)
-                self.stored_tool_orientations[k] = q
-            else:
-                self.stored_tool_orientations[k] = v
-
-    def store_current_tool_orientations(self, tool_names: Optional[List]):
-        self.stored_tool_orientations = {n: self.get_orientation(n) for n in tool_names}
-
-    def merge_tool_orientations_with_defaults(self, tool_names: List[str],
-                                              preferred_tool_orientations: Optional[List] = STORED_ORIENTATION):
-        # If preferred_tool_orientations is None, we use the stored ones as a fallback
-        if preferred_tool_orientations == STORED_ORIENTATION:
-            preferred_tool_orientations = []
-            if self.stored_tool_orientations is None:
-                logfatal(store_error_msg)
-            for k in tool_names:
-                if k not in self.stored_tool_orientations:
-                    rospy.logerr(f"tool {k} has no stored orientation. aborting.")
-                    return []
-                preferred_tool_orientations.append(self.stored_tool_orientations[k])
-        return preferred_tool_orientations
-
-    def follow_jacobian_to_position_from_scene_and_state(self,
-                                                         scene_msg: PlanningScene,
-                                                         joint_state: JointState,
-                                                         group_name: str,
-                                                         tool_names: List[str],
-                                                         points: List[List],
-                                                         preferred_tool_orientations: Optional = STORED_ORIENTATION,
-                                                         vel_scaling=0.1,
-                                                         stop_condition: Optional[Callable] = None,
-                                                         ):
-        if isinstance(tool_names, str):
-            err_msg = "you need to pass in a list of strings, not a single string."
-            rospy.logerr(err_msg)
-            raise ValueError(err_msg)
-
-        preferred_tool_orientations = self.merge_tool_orientations_with_defaults(tool_names,
-                                                                                 preferred_tool_orientations)
-        if len(preferred_tool_orientations) == 0:
-            return ExecutionResult(trajectory=None,
-                                   execution_result=None,
-                                   action_client_state=self.arms_client.get_state(),
-                                   success=False)
-
-        robot_trajectory_msg: RobotTrajectory
-        reached: bool
-        scene_msg, robot_state = merge_joint_state_and_scene_msg(scene_msg, joint_state)
-        robot_trajectory_msg, reached = self.jacobian_follower.plan(
-            group_name=group_name,
-            tool_names=tool_names,
-            preferred_tool_orientations=preferred_tool_orientations,
-            start_state=robot_state,
-            scene=scene_msg,
-            grippers=points,
-            max_velocity_scaling_factor=vel_scaling,
-            max_acceleration_scaling_factor=0.1,
-        )
-
-        planning_success = reached
-        planning_result = PlanningResult(success=planning_success, plan=robot_trajectory_msg)
-        if self.raise_on_failure and not planning_success:
-            raise RobotPlanningError(f"Tried to execute a jacobian action which could not be reached")
-
-        execution_result = self.follow_arms_joint_trajectory(robot_trajectory_msg.joint_trajectory,
-                                                             stop_condition=stop_condition)
-        return PlanningAndExecutionResult(planning_result, execution_result)
-
-    def follow_jacobian_to_position(self,
-                                    group_name: str,
-                                    tool_names: List[str],
-                                    points: List[List],
-                                    preferred_tool_orientations: Optional[List] = STORED_ORIENTATION,
-                                    vel_scaling=0.1,
-                                    stop_condition: Optional[Callable] = None,
-                                    ):
-        if isinstance(tool_names, str):
-            err_msg = "you need to pass in a list of strings, not a single string."
-            rospy.logerr(err_msg)
-            raise ValueError(err_msg)
-
-        preferred_tool_orientations = self.merge_tool_orientations_with_defaults(tool_names,
-                                                                                 preferred_tool_orientations)
-        if len(preferred_tool_orientations) == 0:
-            return ExecutionResult(trajectory=None,
-                                   execution_result=None,
-                                   action_client_state=self.arms_client.get_state(),
-                                   success=False)
-
-        robot_trajectory_msg: RobotTrajectory
-        reached: bool
-        robot_trajectory_msg, reached = self.jacobian_follower.plan(
-            group_name=group_name,
-            tool_names=tool_names,
-            preferred_tool_orientations=preferred_tool_orientations,
-            grippers=points,
-            max_velocity_scaling_factor=vel_scaling,
-            max_acceleration_scaling_factor=0.1,
-        )
-        planning_success = reached
-        planning_result = PlanningResult(success=planning_success, plan=robot_trajectory_msg)
-        if self.raise_on_failure and not planning_success:
-            raise RobotPlanningError(f"Jacobian planning failed")
-
-        execution_result = self.follow_arms_joint_trajectory(robot_trajectory_msg.joint_trajectory,
-                                                             stop_condition=stop_condition)
-        return PlanningAndExecutionResult(planning_result, execution_result)
-
     def get_joint_names(self, group_name: Optional[str] = None):
         # NOTE: Getting a list of joint names should not require anything besides a lookup on the ros parameter server.
         #   However, MoveIt does not have this implemented that way, it requires connecting to the move group node.