diff --git a/demo/scripts/grasp.py b/demo/scripts/grasp.py deleted file mode 100644 index cf313acc..00000000 --- a/demo/scripts/grasp.py +++ /dev/null @@ -1,46 +0,0 @@ -#! /usr/bin/env python -# -*- coding: utf-8 -*- - -from std_msgs.msg import Header -from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3, PoseStamped, Pose, Point, Quaternion -from moveit.task_constructor import core, stages -import moveit_commander -import rospy -import numpy - -from moveit.python_tools import roscpp_init -roscpp_init("mtc_tutorial") -rospy.init_node('mtc_tutorial_py', anonymous=False) - -def create_object(): - scene = moveit_commander.PlanningSceneInterface(synchronous=True) - scene.remove_world_object(); - pose = PoseStamped() - pose.header.frame_id = "world" - pose.pose.position.x = 0.5 - pose.pose.position.y = numpy.random.uniform(-0.1, 0.1) - pose.pose.position.z = 0.16 - pose.pose.orientation.w = 1.0 - scene.add_box('object', pose, size=(0.05, 0.05, 0.2)) - -group = "panda_arm" -eef_frame = "panda_link8" - -# Cartesian interpolation planner -cartesian = core.CartesianPath() - -task = core.Task() - -# start from current robot state -task.add(stages.CurrentState("current state")) - -move = stages.MoveTo("to object", cartesian) -move.group = group -header = Header(frame_id = "object") -move.setGoal(PoseStamped(header=header, pose=Pose(position=Point(0,0,0.18),orientation=Quaternion(0.92388, -0.38268, 0, 0)))) -task.add(move) - -create_object() -if task.plan(): - task.publish(task.solutions[0]) -rospy.spin() diff --git a/demo/scripts/pick.py b/demo/scripts/pick.py deleted file mode 100644 index f83a4f5e..00000000 --- a/demo/scripts/pick.py +++ /dev/null @@ -1,65 +0,0 @@ -#! /usr/bin/env python -# -*- coding: utf-8 -*- - -from std_msgs.msg import Header -from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3, PoseStamped, Pose, Point, Quaternion -from moveit.task_constructor import core, stages -import moveit_commander -import rospy -import numpy - -from moveit.python_tools import roscpp_init -roscpp_init("mtc_tutorial") -rospy.init_node('mtc_tutorial_py', anonymous=False) - -group = 'panda_arm' -eef = 'panda_hand' -eef_frame = "panda_link8" - -sampling_planner = core.JointInterpolationPlanner() -cartesian_planner = core.CartesianPath() - -task = core.Task() - -task.properties.update({'group': group, 'eef': eef, 'hand': eef, 'hand_grasping_frame': eef, 'ik_frame': eef_frame}) - -currstate = stages.CurrentState('current state') -#task.add(currstate) # Adding it to the task results in error for argument types in setMonitoredStage in GenerateGraspPose - -open_hand = stages.MoveTo("open hand", sampling_planner) -open_hand.group = eef -open_hand.setGoal('open') -task.add(open_hand) - -connect = stages.Connect('move to pick', [(group, sampling_planner)]) -connect.timeout = 5 -connect.properties.configureInitFrom(core.PARENT) -task.add(connect) - -grasp = core.SerialContainer('pick object') -task.properties.exposeTo(grasp.properties, ['eef', 'hand', 'group', 'ik_frame']) -grasp.properties.configureInitFrom(core.PARENT, ['eef', 'hand', 'group', 'ik_frame']) - -approach_object = stages.MoveRelative("approach_object", cartesian_planner) -approach_object.properties.update({'marker_ns': 'approach_object', 'link': eef_frame}) -approach_object.properties.configureInitFrom(core.PARENT, ['group']) -approach_object.min_distance = 0.01 -approach_object.max_distance = 0.1 -print(approach_object.properties.__getitem__('group')) # Why is this None? how to get properties from within SerialContainer? -approach_object.setDirection(TwistStamped(header=Header(frame_id = eef_frame), twist=Twist(linear=Vector3(0,0,0.1)))) -grasp.insert(approach_object) - -generatepose = stages.GenerateGraspPose('generate grasp pose') -generatepose.properties.configureInitFrom(core.PARENT) -generatepose.properties.update({'marker_ns': 'grasp_pose'}) -generatepose.pregrasp = 'open' -generatepose.object = 'base' -generatepose.angle_delta = numpy.pi/12 -generatepose.setMonitoredStage(currstate) - -# To be continued -task.add(grasp) - -if task.plan(): - task.publish(task.solutions[0]) -rospy.spin() diff --git a/notes.org b/notes.org deleted file mode 100644 index eb6bac4c..00000000 --- a/notes.org +++ /dev/null @@ -1,53 +0,0 @@ -* scheduling -There are 2 levels of scheduling: -- choosing which state or state pair should be scheduled for execution -- choosing which stage should be scheduled for execution - -Main goal is to find small-cost solutions fast. -To this end, we need to find a solution at all, i.e. connect start to end. -Second, we prefer small-cost solutions. - -First sort by length of trajectory, second by sum of trajectory costs. - -** approach -- scheduling priority for stage (default / assigned by user) -- fallback: sort stages by priority -- evaluate success rate -- expected costs of solutions -- expected computing time -- Container::schedule() -> return ordered list of stages to execute - -* containers / interfaces -Basic stages have unique solutions connecting start-end. -Containers allow for several solutions connecting start-end: -- serial: several pathes might exist -- parallel: inherent - -* notation -push vs pull connections - -* TODO -** modify-ps -- std::function API -- replace templates with sfniae - -** eef collision -- requires modification in MoveIt -- disable warnings vs. only consider relevant links (separate function?) - -** MoveTo / MoveRelative -- interface: pass goal constraints -- fill goal constraints during setup? - requires transforms to be specified relative to current robot pose - -** properties -- fix plan_pick_pa10: forward grasp property -- global type registry for serialization/deserialization functions -- use bits as enums to allow for configureInitFrom(PARENT | INTERFACE) -** incremental GraspPoseGenerator -in pending_ list, store the current angular value -** TaskModels -- move scene_, display_context_ to BaseTaskModel -- use registered property creators for RemoteTaskModel too - - deserialize property - - call creator function diff --git a/pick+place.org b/pick+place.org deleted file mode 100644 index f30f8d6b..00000000 --- a/pick+place.org +++ /dev/null @@ -1,28 +0,0 @@ -* capability params -- overall planning timeout -- max number of solutions - -** Pick -- possible_grasps -> fixed list -- instantiate GraspProvider+Task on each call (eventually cache) -- object -- key-value parameter map (passed on to Action Server) - -* GraspProvider params -- eef (hand_group, arm_group, parent link) - - hand_group: default from arm_group - - arm_group: default from hand_group - - grasp frame (PoseStamped rel. zu Link fest zu Tip-Link im Arm) -- key-value parameter map (passed on to Action Server) - - grasp config - - action server name (with default) - -* PlaceProvider params -- PlaceLocation[] -- surface_name: sample PlaceLocations, disable collision with table during IK - -* GraspMsg -- grasp frame? -- no-collision links (during closing) -- no-collision - diff --git a/test.py b/test.py deleted file mode 100644 index 368ddcd8..00000000 --- a/test.py +++ /dev/null @@ -1,16 +0,0 @@ -import moveit.task_constructor as mtc - -o=mtc.MyObject(42) -a=o -#print o -#print a -mtc.access(o) -mtc.access(o) -mtc.consume(o) -mtc.access(o) -mtc.access(o) -#print o -#print a -del o -del a -print "done"