mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Remove accidentally committed files
This commit is contained in:
parent
1f8dae86dd
commit
a939358d40
@ -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()
|
||||
@ -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()
|
||||
53
notes.org
53
notes.org
@ -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
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user