diff --git a/.gitignore b/.gitignore
index 4c96b68..0c147a0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -26,6 +26,7 @@ share/python-wheels/
.installed.cfg
*.egg
MANIFEST
+.vscode/
# PyInstaller
# Usually these files are written by a python script from a template
diff --git a/Dockerfile b/Dockerfile
new file mode 100755
index 0000000..73c505c
--- /dev/null
+++ b/Dockerfile
@@ -0,0 +1,42 @@
+FROM osrf/ros:noetic-desktop-full
+
+# Install apt packages
+RUN apt update && apt install -y git nano
+RUN apt install ros-noetic-tf2-tools -y
+RUN apt install python3-pip python3-tk -y
+
+# install python dependencies to run frankapy within the docker container
+RUN pip3 install autolab_core
+RUN pip3 install --force-reinstall pillow==9.0.1 && pip3 install --force-reinstall scipy==1.8
+RUN pip3 install numpy-quaternion numba && pip3 install --upgrade google-api-python-client
+RUN pip3 install --force-reinstall numpy==1.23.5
+
+# Install moveit and franka_ros
+RUN apt install ros-noetic-moveit ros-noetic-franka-ros -y
+
+# Make src/git_packages and clone panda_moveit_config
+RUN mkdir -p /home/ros_ws/src/git_packages && \
+ cd /home/ros_ws/src/git_packages && \
+ git clone https://github.com/ros-planning/panda_moveit_config.git -b noetic-devel
+
+# Copy the frankapy folder into the container
+COPY frankapy /home/ros_ws/src/git_packages/frankapy/frankapy
+COPY catkin_ws /home/ros_ws/src/git_packages/frankapy/catkin_ws
+
+# Copy src folder from desktop to /home/ros_ws/src/devel_packages (this is where we will put our custom packages)
+COPY moveit_frankapy/src/devel_packages /home/ros_ws/src/devel_packages
+
+# Install dependencies using rosdep
+RUN /bin/bash -c "source /opt/ros/noetic/setup.bash; cd /home/ros_ws; rosdep install --from-paths src --ignore-src -r -y"
+
+# add ros workspace to bashrc
+RUN echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
+RUN echo "source /home/ros_ws/devel/setup.bash" >> ~/.bashrc
+
+# build workspace
+RUN /bin/bash -c "source /opt/ros/noetic/setup.bash; cd /home/ros_ws; catkin_make"
+
+# set workdir as home/ros_ws
+WORKDIR /home/ros_ws
+
+CMD [ "bash" ]
\ No newline at end of file
diff --git a/moveit_frankapy/.gitignore b/moveit_frankapy/.gitignore
new file mode 100644
index 0000000..259148f
--- /dev/null
+++ b/moveit_frankapy/.gitignore
@@ -0,0 +1,32 @@
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
diff --git a/moveit_frankapy/README.md b/moveit_frankapy/README.md
new file mode 100644
index 0000000..f7fb90d
--- /dev/null
+++ b/moveit_frankapy/README.md
@@ -0,0 +1,46 @@
+# moveit_frankapy
+
+Prerequisites:
+
+- Install Docker: https://docs.docker.com/engine/install/ubuntu/
+
+- (Optional) Add docker to sudoers: https://docs.docker.com/engine/install/linux-postinstall/. If not done, please run all docker commands with a ```sudo```
+
+
+
+
+Steps to run moveit with frankapy
+Frankapy Setup
+- Clone repo: git clone --recursive https://github.com/vib2810/frankapy.git
+- bash start_control_pc.sh -u student -i iam-snowwhite
+
+1. Build the Docker Container:
+
+ ```
+ docker build -t moveit_frankapy .
+ ```
+
+2. Run Docker Container:
+ ```
+ cd moveit_frankapy
+ bash run_docker.sh
+ ```
+
+ Run A Terminal connected to the Docker Container:
+ ```
+ cd moveit_frankapy
+ bash terminal_docker.sh
+ ```
+
+3. Run the MoveIt launch file
+ In a terminal connected to the Docker Container:
+ ```
+ roslaunch manipulation demo_frankapy.launch
+ ```
+
+4. Run the demo_moveit.py script
+ In a terminal connected to the Docker Container:
+ ```
+ rosrun manipulation demo_moveit.py
+ ```
+
diff --git a/moveit_frankapy/run_docker.sh b/moveit_frankapy/run_docker.sh
new file mode 100755
index 0000000..e3b4903
--- /dev/null
+++ b/moveit_frankapy/run_docker.sh
@@ -0,0 +1,14 @@
+xhost +local:root
+docker container prune -f
+docker run --privileged --rm -it \
+ --name="moveit_frankapy" \
+ --env="DISPLAY=$DISPLAY" \
+ --env="QT_X11_NO_MITSHM=1" \
+ --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
+ --volume="$XAUTH:$XAUTH" \
+ --network host \
+ -v $(pwd)/src/devel_packages:/home/ros_ws/src/devel_packages \
+ moveit_frankapy \
+ bash
+
+# NOTE: --network host is used to allow the container to access the host's network
diff --git a/moveit_frankapy/src/devel_packages/manipulation/CMakeLists.txt b/moveit_frankapy/src/devel_packages/manipulation/CMakeLists.txt
new file mode 100644
index 0000000..d8fae66
--- /dev/null
+++ b/moveit_frankapy/src/devel_packages/manipulation/CMakeLists.txt
@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(manipulation)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+ rospy
+ std_msgs
+)
+
+## Declare a catkin package
+catkin_package()
+
+## Specify locations of header files
+include_directories(
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Add executables
+catkin_install_python(PROGRAMS
+ src/demo_moveit.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+catkin_install_python(PROGRAMS
+ src/robot_joint_converter.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark other files for installation (e.g. launch and config files)
+install(DIRECTORY launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
\ No newline at end of file
diff --git a/moveit_frankapy/src/devel_packages/manipulation/launch/demo_frankapy.launch b/moveit_frankapy/src/devel_packages/manipulation/launch/demo_frankapy.launch
new file mode 100644
index 0000000..044f328
--- /dev/null
+++ b/moveit_frankapy/src/devel_packages/manipulation/launch/demo_frankapy.launch
@@ -0,0 +1,70 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ [real_robot_joints]
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/moveit_frankapy/src/devel_packages/manipulation/package.xml b/moveit_frankapy/src/devel_packages/manipulation/package.xml
new file mode 100644
index 0000000..9a4bf49
--- /dev/null
+++ b/moveit_frankapy/src/devel_packages/manipulation/package.xml
@@ -0,0 +1,23 @@
+
+
+ manipulation
+ 0.0.1
+
+ A package which allows the use of moveit with frankapy.
+
+
+ Vibhakar Mohta
+ MIT
+
+ catkin
+
+ rospy
+ std_msgs
+
+ rospy
+ std_msgs
+
+
+ catkin
+
+
\ No newline at end of file
diff --git a/moveit_frankapy/src/devel_packages/manipulation/src/demo_moveit.py b/moveit_frankapy/src/devel_packages/manipulation/src/demo_moveit.py
new file mode 100644
index 0000000..e4f5980
--- /dev/null
+++ b/moveit_frankapy/src/devel_packages/manipulation/src/demo_moveit.py
@@ -0,0 +1,326 @@
+# Author: Vibhakar Mohta vib2810@gmail.com
+#!/usr/bin/env python3
+import sys
+import rospy
+import moveit_commander
+import moveit_msgs.msg
+import geometry_msgs.msg
+import sensor_msgs.msg
+import numpy as np
+from moveit_msgs.msg import PlanningScene, CollisionObject
+from shape_msgs.msg import SolidPrimitive, Mesh
+import scipy.spatial.transform as spt
+
+sys.path.append("/home/ros_ws/src/git_packages/frankapy")
+from frankapy import FrankaArm, SensorDataMessageType
+from frankapy import FrankaConstants as FC
+from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg
+from frankapy.proto import JointPositionSensorMessage, ShouldTerminateSensorMessage
+from franka_interface_msgs.msg import SensorDataGroup
+
+class moveit_planner():
+ def __init__(self) -> None: #None means no return value
+ moveit_commander.roscpp_initialize(sys.argv)
+ rospy.init_node('move_group_python_interface_tutorial',anonymous=True)
+
+ self.robot = moveit_commander.RobotCommander()
+ self.scene = moveit_commander.PlanningSceneInterface()
+ group_name = "panda_arm"
+ self.group = moveit_commander.MoveGroupCommander(group_name)
+ self.group.set_end_effector_link("panda_hand")
+ self.obs_pub = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10)
+
+ # used to visualize the planned path
+ self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory,queue_size=20)
+
+ planning_frame = self.group.get_planning_frame()
+ eef_link = self.group.get_end_effector_link()
+
+ print("---------Moveit Planner Class Initialized---------")
+ print("Planning frame: ", planning_frame)
+ print("End effector: ", eef_link)
+ print("Robot Groups:", self.robot.get_group_names())
+
+ # frankapy
+ self.pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000)
+ self.fa = FrankaArm(init_node = False)
+
+ # Utility Functions
+ def print_robot_state(self):
+ print("Joint Values:\n", self.group.get_current_joint_values())
+ print("Pose Values (panda_hand):\n", self.group.get_current_pose())
+ print("Pose Values (panda_end_effector):\nNOTE: Quaternion is (w,x,y,z)\n", self.fa.get_pose())
+
+ def reset_joints(self):
+ self.fa.reset_joints()
+
+ def goto_joint(self, joint_goal):
+ self.fa.goto_joints(joint_goal, duration=5, dynamic=True, buffer_time=10)
+
+ def get_plan_given_pose(self, pose_goal: geometry_msgs.msg.Pose):
+ """
+ Plans a trajectory given a tool pose goal
+ Returns joint_values
+ joint_values: numpy array of shape (N x 7)
+ """
+ output = self.group.plan(pose_goal)
+ plan = output[1]
+ joint_values = []
+ for i in range(len(plan.joint_trajectory.points)):
+ joint_values.append(plan.joint_trajectory.points[i].positions)
+ joint_values = np.array(joint_values)
+ return joint_values
+
+ def get_plan_given_joint(self, joint_goal_list):
+ """
+ Plans a trajectory given a joint goal
+ Returns joint_values and moveit plan
+ joint_values: numpy array of shape (N x 7)
+ plan: moveit_msgs.msg.RobotTrajectory object
+ """
+ joint_goal = sensor_msgs.msg.JointState()
+ joint_goal.name = ["panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7"]
+ joint_goal.position = joint_goal_list
+
+ output = self.group.plan(joint_goal)
+ plan = output[1]
+ joint_values = []
+ for i in range(len(plan.joint_trajectory.points)):
+ joint_values.append(plan.joint_trajectory.points[i].positions)
+ joint_values = np.array(joint_values)
+ return joint_values, plan
+
+ def execute_plan(self, joints_traj):
+ """
+ joints_traj shape: (N x 7)
+ """
+ # interpolate the trajectory
+ num_interp_slow = 50 # number of points to interpolate for the start and end of the trajectory
+ num_interp = 20 # number of points to interpolate for the middle part of the trajectory
+ interpolated_traj = []
+ t_linear = np.linspace(1/num_interp, 1, num_interp)
+ t_slow = np.linspace(1/num_interp_slow, 1, num_interp_slow)
+ t_ramp_up = t_slow**2
+ t_ramp_down = 1 - (1-t_slow)**2
+
+ interpolated_traj.append(joints_traj[0,:])
+ for t_i in range(len(t_ramp_up)):
+ dt = t_ramp_up[t_i]
+ interp_traj_i = joints_traj[1,:]*dt + joints_traj[0,:]*(1-dt)
+ interpolated_traj.append(interp_traj_i)
+
+ for i in range(2, joints_traj.shape[0]-1):
+ for t_i in range(len(t_linear)):
+ dt = t_linear[t_i]
+ interp_traj_i = joints_traj[i,:]*dt + joints_traj[i-1,:]*(1-dt)
+ interpolated_traj.append(interp_traj_i)
+
+ for t_i in range(len(t_ramp_down)):
+ dt = t_ramp_down[t_i]
+ interp_traj_i = joints_traj[-1,:]*dt + joints_traj[-2,:]*(1-dt)
+ interpolated_traj.append(interp_traj_i)
+
+ interpolated_traj = np.array(interpolated_traj)
+
+ print('Executing joints trajectory of shape: ', interpolated_traj.shape)
+
+ rate = rospy.Rate(50)
+ # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed
+ self.fa.goto_joints(interpolated_traj[1], duration=5, dynamic=True, buffer_time=20)
+ init_time = rospy.Time.now().to_time()
+ for i in range(2, interpolated_traj.shape[0]):
+ traj_gen_proto_msg = JointPositionSensorMessage(
+ id=i, timestamp=rospy.Time.now().to_time() - init_time,
+ joints=interpolated_traj[i]
+ )
+ ros_msg = make_sensor_group_msg(
+ trajectory_generator_sensor_msg=sensor_proto2ros_msg(
+ traj_gen_proto_msg, SensorDataMessageType.JOINT_POSITION)
+ )
+ self.pub.publish(ros_msg)
+ rate.sleep()
+
+ # Stop the skill
+ # Alternatively can call fa.stop_skill()
+ term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True)
+ ros_msg = make_sensor_group_msg(
+ termination_handler_sensor_msg=sensor_proto2ros_msg(
+ term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE)
+ )
+ self.pub.publish(ros_msg)
+
+ # Test Functions
+ def unit_test_joint(self, execute = False, guided = False):
+ """
+ Unit test for joint trajectory planning
+ Resets to home and plans to the joint goal
+ Displays the planned path to a fixed joint goal on rviz
+
+ Parameters
+ ----------
+ execute: bool
+ If True, executes the planned trajectory
+ guided: bool
+ If True, runs guide mode where the user can move the robot to a desired joint goal
+ Else, uses a fixed joint goal
+ """
+ if guided:
+ print("Running Guide Mode, Move Robot to Desired Pose")
+ self.fa.run_guide_mode(10, block=True)
+ self.fa.stop_skill()
+ joint_goal_list = self.fa.get_joints()
+ print("Joint Goal: ", joint_goal_list)
+ else:
+ # A random joint goal
+ joint_goal_list = [6.14813255e-02 ,4.11382927e-01, 6.80023936e-02,-2.09547337e+00,-2.06094866e-03,2.56799173e+00, 9.20088362e-01]
+ print("Resetting Joints")
+ self.fa.reset_joints()
+ plan_joint_vals, plan_joint = self.get_plan_given_joint(joint_goal_list)
+ print("Planned Path Shape: ", plan_joint_vals.shape)
+ if execute:
+ print("Executing Plan")
+ self.execute_plan(plan_joint_vals)
+
+ def unit_test_pose(self, execute = False, guided = False):
+ """
+ Unit test for pose trajectory planning
+ Resets to home and plans to the pose goal
+ Displays the planned path to a fixed pose goal on rviz
+
+ Parameters
+ ----------
+ execute: bool
+ If True, executes the planned trajectory
+ guided: bool
+ If True, runs guide mode where the user can move the robot to a desired pose goal
+ Else, uses a fixed pose goal
+ """
+ print("Unit Test for Tool Pose Trajectory Planning")
+ if guided:
+ print("Running Guide Mode, Move Robot to Desired Pose")
+ self.fa.run_guide_mode(10, block=True)
+ self.fa.stop_skill()
+ pose_goal_fa = self.fa.get_pose()
+ pose_goal = geometry_msgs.msg.Pose()
+ pose_goal.position.x = pose_goal_fa.translation[0]
+ pose_goal.position.y = pose_goal_fa.translation[1]
+ pose_goal.position.z = pose_goal_fa.translation[2]
+ pose_goal.orientation.w = pose_goal_fa.quaternion[0]
+ pose_goal.orientation.x = pose_goal_fa.quaternion[1]
+ pose_goal.orientation.y = pose_goal_fa.quaternion[2]
+ pose_goal.orientation.z = pose_goal_fa.quaternion[3]
+ else:
+ pose_goal = geometry_msgs.msg.Pose()
+ # a random test pose
+ pose_goal.position.x = 0.5843781940153249
+ pose_goal.position.y = 0.05791107711908864
+ pose_goal.position.z = 0.23098061041636195
+ pose_goal.orientation.x = -0.9186984147774666
+ pose_goal.orientation.y = 0.3942492534293267
+ pose_goal.orientation.z = -0.012441904611284204
+ pose_goal.orientation.w = 0.020126567105018894
+
+ # Convert to moveit pose
+ pose_goal = self.get_moveit_pose_given_frankapy_pose(pose_goal)
+ print("Pose Goal: ", pose_goal)
+ print("Resetting Joints")
+ self.fa.reset_joints()
+ plan_pose = self.get_plan_given_pose(pose_goal)
+ print("Planned Path Shape: ", plan_pose.shape)
+ if execute:
+ print("Executing Plan")
+ self.execute_plan(plan_pose)
+
+ def get_moveit_pose_given_frankapy_pose(self, pose):
+ """
+ Converts a frankapy pose (in panda_end_effector frame) to a moveit pose (in panda_hand frame)
+ by adding a 10 cm offset to z direction
+ """
+ transform_mat = np.array([[1,0,0,0],
+ [0,1,0,0],
+ [0,0,1,-0.1034],
+ [0,0,0,1]])
+ pose_mat = self.pose_to_transformation_matrix(pose)
+ transformed = pose_mat @ transform_mat
+ pose_goal = self.transformation_matrix_to_pose(transformed)
+ return pose_goal
+
+ def pose_to_transformation_matrix(self, pose):
+ """
+ Converts geometry_msgs/Pose to a 4x4 transformation matrix
+ """
+ T = np.eye(4)
+ T[0,3] = pose.position.x
+ T[1,3] = pose.position.y
+ T[2,3] = pose.position.z
+ r = spt.Rotation.from_quat([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
+ T[0:3, 0:3] = r.as_matrix()
+ return T
+
+ def transformation_matrix_to_pose(self, trans_mat):
+ """
+ Converts a 4x4 transformation matrix to geometry_msgs/Pose
+ """
+ out_pose = geometry_msgs.msg.Pose()
+ out_pose.position.x = trans_mat[0,3]
+ out_pose.position.y = trans_mat[1,3]
+ out_pose.position.z = trans_mat[2,3]
+
+ #convert rotation matrix to quaternion
+ r = spt.Rotation.from_matrix(trans_mat[0:3, 0:3])
+ quat = r.as_quat()
+ out_pose.orientation.x = quat[0]
+ out_pose.orientation.y = quat[1]
+ out_pose.orientation.z = quat[2]
+ out_pose.orientation.w = quat[3]
+ return out_pose
+
+ def add_box(self, name, pose: geometry_msgs.msg.PoseStamped(), size):
+ """
+ Adds a collision box to the planning scene
+
+ Parameters
+ ----------
+ name : str
+ Name of the box
+ pose : geometry_msgs.msg.PoseStamped
+ Pose of the box (Centroid and Orientation)
+ size : list
+ Size of the box in x, y, z
+ """
+ self.scene.add_box(name, pose, size)
+
+ def remove_box(self, name):
+ self.scene.remove_world_object(name)
+
+if __name__ == "__main__":
+ franka_moveit = moveit_planner()
+
+ # Print Current Robot State (Joint Values and End Effector Pose)
+ franka_moveit.print_robot_state()
+
+ # Test Planning
+ # To execute the plan, set execute = True
+ # To plan to a joint goal using run_guide_mode, set guided = True
+
+ # Test Joint Planning
+ franka_moveit.unit_test_joint(execute=False, guided=False)
+
+ # Test Tool Position Planning
+ # franka_moveit.unittest_pose(execute=True, guided=True)
+
+ # Adding and removing obstacle boxes to planning scene
+ # box_pose = geometry_msgs.msg.PoseStamped()
+ # box_pose.header.frame_id = "panda_link0"
+ # box_pose.pose.position.x = 0.5767154
+ # box_pose.pose.position.y = -0.17477638
+ # box_pose.pose.position.z = 0.00872429
+ # box_pose.pose.orientation.x = 0.98198148
+ # box_pose.pose.orientation.y = 0.18717703
+ # box_pose.pose.orientation.z = 0.02544852
+ # box_pose.pose.orientation.w = -0.00495174
+ # franka_moveit.add_box("box", box_pose, [0.02, 0.065, 0.015])
+
+ # Remove added obstacle box
+ # franka_moveit.remove_box("box")
+
diff --git a/moveit_frankapy/src/devel_packages/manipulation/src/robot_joint_converter.py b/moveit_frankapy/src/devel_packages/manipulation/src/robot_joint_converter.py
new file mode 100644
index 0000000..307ffed
--- /dev/null
+++ b/moveit_frankapy/src/devel_packages/manipulation/src/robot_joint_converter.py
@@ -0,0 +1,35 @@
+# Author: Vibhakar Mohta vib2810@gmail.com
+# Node reads current robot joint state
+# - Reads manipulator joint state using ros service call through frankapy
+# - Reads gripper joint state by subscribing to /franka_gripper_1/joint_states
+# Node publishes the robot joints to /real_robot_joints to be used by robot_state_publisher and eventually moveit
+
+#!/usr/bin/env python3
+import sys
+import rospy
+import sensor_msgs.msg
+sys.path.append("/home/ros_ws/src/git_packages/frankapy")
+from frankapy import FrankaArm
+
+def gripper_callback(msg):
+ global gripper_width
+ gripper_width = msg.position[0]
+
+if __name__ == "__main__":
+ global gripper_width
+ gripper_width = 0
+ rospy.init_node('move_group_python_interface_tutorial',anonymous=True)
+ fa = FrankaArm(init_node = False)
+ pub = rospy.Publisher("/real_robot_joints", sensor_msgs.msg.JointState, queue_size=10)
+ gripper_sub = rospy.Subscriber("/franka_gripper_1/joint_states", sensor_msgs.msg.JointState, gripper_callback)
+ rate = rospy.Rate(10)
+ msg = sensor_msgs.msg.JointState()
+ msg.name = ["panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7", "panda_finger_joint1", "panda_finger_joint2"]
+
+ while not rospy.is_shutdown():
+ state = fa.get_joints().tolist()
+ msg.header.stamp = rospy.Time.now()
+ state.append(gripper_width); state.append(gripper_width)
+ msg.position = state
+ pub.publish(msg)
+ rate.sleep()
\ No newline at end of file
diff --git a/moveit_frankapy/src/devel_packages/run_guide_mode.py b/moveit_frankapy/src/devel_packages/run_guide_mode.py
new file mode 100644
index 0000000..eaf632f
--- /dev/null
+++ b/moveit_frankapy/src/devel_packages/run_guide_mode.py
@@ -0,0 +1,41 @@
+"""
+Usage:
+python run_guide_mode.py
+Commands:
+1-> Print Tool Pose
+2-> Print Joint Pose
+3-> Stop Skill and Exit
+4-> Stop Skill and Reset to Home Position
+"""
+import numpy as np
+import time
+import sys
+sys.path.append("/home/ros_ws/src/git_packages/frankapy")
+from frankapy import FrankaArm
+if __name__ == "__main__":
+
+ start = time.time()
+ fa = FrankaArm()
+ fa.run_guide_mode(10000,block=False)
+
+ while((time.time()-start) < 10000):
+ input_num = int(input("Enter a number: "))
+ if (input_num==1):
+ T_ee_world = fa.get_pose()
+ print("pose: ")
+ print(T_ee_world)
+ time.sleep(0.01)
+ if(input_num==2):
+ joints = fa.get_joints()
+ print("joints: ")
+ print(joints)
+ time.sleep(0.01)
+ if(input_num==3):
+ fa.stop_skill()
+ break
+ if(input_num==4):
+ fa.stop_skill()
+ fa.reset_joints()
+ break
+
+ print("done")
\ No newline at end of file
diff --git a/moveit_frankapy/terminal_docker.sh b/moveit_frankapy/terminal_docker.sh
new file mode 100755
index 0000000..4d70947
--- /dev/null
+++ b/moveit_frankapy/terminal_docker.sh
@@ -0,0 +1,2 @@
+xhost +local:docker
+docker exec -it moveit_frankapy bash
\ No newline at end of file