Interface Sketches

Base Functionality

Basic Plan and Execute Goal

from moveit_py.core import RobotState
from moveit_py.planning import MoveIt


#######################################
# MoveIt Setup
#######################################

moveit = MoveIt(node_name="moveit_py", 
                config=default_config #this should be the default config provided by MoveIt, optionally can be overridden by user from launch params,
                robot="pr2" # option to override robot config -> requires moveit_resources package format,
                planning_pipeline= # option to override planning pipeline settings -> expects dict,
                plan_request_params= # option to override plan params -> expects dict, 
                planning_scene_monitor_options= # option to override planning scene monitor options -> expects dict,
                provide_planning_scene_service=True,
                ... 
                )

# Add functionality to dynamically update node parameters such that we can alter the above configuration.

#######################################
# Selecting Planning Component
####################################### 
# look to add support for dynamically specifying new JointModelGroup
moveit.get_planning_component("panda_arm")
 

#######################################
# Setting Start State
#######################################

# 1. set start state by name
panda_arm.set_start_state("ready")

# 2. set start state with pose
pose = PoseStamped()
pose_start.header.frame_id = "panda_link0"
pose_start.pose.orientation.w = 1.0
pose_start.pose.position.x = 0.28
pose_start.pose.position.y = -0.2
pose_start.pose.position.z = 0.5
panda_arm.set_start_state(pose, "panda_link8")

# 3. set start state by current state
panda_arm.set_start_to_current_state()

# 4. set start state with RobotState (Not Recommended)
robot_start_state = RobotState(robot_model=moveit.robot)
# from here set RobotState in various ways e.g. from IK
panda_arm.set_start_state(robot_start_state)


#######################################
# Setting Goal Pose
#######################################

# 1. set goal by state name
panda_arm.set_goal("extended")

# 2. set pose goal (add support for specifying via dict)
pose_goal = PoseStamped()
pose_goal.header.frame_id = "panda_link0"
pose_goal.pose.orientation.w = 1.0
pose_goal.pose.position.x = 0.28
pose_goal.pose.position.y = -0.2
pose_goal.pose.position.z = 0.5
panda_arm.set_goal(pose_goal, "panda_link8")

# 3. set goal from RobotState (Not Recommended)
robot_state = RobotState()
panda_arm.set_goal(robot_state)

# 4. Add support to specify multiple goals through lists of the above objects
panda_arm.set_goals(["extended", {pose: pose_goal, link: "panda_link8"}, robot_state])

#######################################
# Planning and Execution
#######################################

# plan to goal
success = panda_arm.plan_and_execute(consecutive=True) # default to planning and executing all set goals consecutively

Modify Planning Scene

from moveit_py.core import PlanningScene

Forward and Inverse Kinematics

from moveit_py.core import RobotModel

Collision Checking

Servo

Servo is not fully thought out yet but a sketch is given below.

from moveit_py.planning import MoveIt
from moveit_py.servo import Servo

servo = Servo(config=servo_config,
      robot="panda",
      planning_frame="panda_link8",
      command_in_type="", 
      command_out_type="",
      planning_component="panda_arm",
      ...
      )

# Once Servo is up and running I wish to specify position/velocity control commands for eef 
# I also wish to add support for gripper commands
 
### Deep learning framework code goes here ###
### Suppose that learnt_controller is a neural network with learnt params ### 
controller = learnt_controller 

while #condition for length of time we wish to servo for:
   state = ... # get environment state
   servo_command = controller(state) # predict servo command
   servo.command(servo_command) # execute servo command via publishing ROS topic

Advanced Functionality

Data Collection

from moveit_py.servo import Servo
from moveit_py.data import RecordData

def main():
   servo = Servo(config=servo_config,
         robot="panda",
         planning_frame="panda_link8",
         command_in_type="", 
         command_out_type="",
         planning_component="panda_arm",
         ...
         )

# Record data node (when user presses button start record and stop when button is pressed again)
   RecordData(config=,
           format="bag",
           name="stack_block_expert_demo",
           user_trigger=True,
           platform=,
           ...
           )

if __name__=="__main__":
   main()

Also include RecordData for a series of planned and executed robot trajectories.

Discussion Point

Most Python users are not familiar with ROS and the build system is uses. How feasible or appropriate would it be to have a Python package that configures ROS nodes and their execution without the user having to deal with these details directly (e.g. instantiating a class will spin up multiple nodes). This will likely require significant work in terms of the python package build and configuring the install of its dependencies (e.g. ROS).