Source-linked AI summary
Object-Centric Task and Motion Planning in Dynamic Environments
Toki Migimatsu, Jeannette Bohg
TL;DR
TAMP plans can become invalid when environments change, while generating trajectories can take minutes. This paper plans over Cartesian frames relative to target objects, and demonstrates the approach on a real robot amid moving objects, noisy perception, and external disturbances.
Problem
TAMP trajectories may take minutes to generate and become invalid when environments change; common methods also assume static objects, accurate perception, and precise commands.
Source: §I · p. 1, §II · p. 2Method
The algorithm optimizes Cartesian frames relative to target objects and integrates the resulting plans with reactive controllers.
Source: §I · p. 1Results
The approach was demonstrated on a real robot with noisy RGB-D perception, moving objects, and external disturbances.
Source: §VIII · p. 7Takeaways & Limitations
Object-relative plans can be integrated with reactive closed-loop controllers that adapt to environmental changes in real time.
Source: §I · p. 1, §VIII · p. 7The method checks only object-level collisions; robot-link collisions and joint limits are left to lower-level planners or controllers.
Source: §VIII · p. 7
Abstract
from arXiv · showhide
We address the problem of applying Task and Motion Planning (TAMP) in real world environments. TAMP combines symbolic and geometric reasoning to produce sequential manipulation plans, typically specified as joint-space trajectories, which are valid only as long as the environment is static and perception and control are highly accurate. In case of any changes in the environment, slow re-planning is required. We propose a TAMP algorithm that optimizes over Cartesian frames defined relative to target objects. The resulting plan then remains valid even if the objects are moving and can be executed by reactive controllers that adapt to these changes in real time. We apply our TAMP framework to a torque-controlled robot in a pick and place setting and demonstrate its ability to adapt to changing environments, inaccurate perception, and imprecise control, both in simulation and the real world.
I. INTRODUCTION
Conventional TAMP produces trajectories that can take minutes to generate and may require replanning when the environment changes. This work plans over object-relative Cartesian frames to work with reactive controllers and support manipulation in changing environments.
I. INTRODUCTION
- Relative Cartesian frames let reactive controllers adapt to changing object poses, perception errors, and imprecise control during manipulation.The framework returns desired object poses at key timepoints rather than full joint-space trajectories.Source: §I · p. 1, §II · p. 2
II. RELATED WORK
- The authors extend Logic Geometric Programming to dynamic environments, optimizing geometric parameters for candidate action skeletons without geometric backtracking.The approach uses nonlinear optimization to consider geometric parameters simultaneously and can encode goals such as minimizing robot energy.Source: §II · p. 2, A. General LGP Formulation · p. 3
- Earlier TAMP methods can take tens of seconds to minutes, and commonly assume static objects, accurate poses, and precise robot commands.Existing methods addressing perception uncertainty still do not handle dynamic environments.Source: §II · p. 2
A. General LGP Formulation
- LGP combines discrete STRIPS planning with continuous trajectory optimization, searching action skeletons and optimizing trajectories to test physical feasibility.The search returns the lowest-objective trajectory found when terminated.Source: A. General LGP Formulation · pp. 2–3
B. Joint Space Formulation
The formulation replaces augmented joint configurations with control-to-target relative poses, making action constraints independent across time. It represents trajectories at key positions and shifts their cross-time dependency to the objective.
B. Joint Space Formulation
- In joint-space KOMO, augmented configurations include the robot and manipulated objects, and their dimension changes as the number of manipulated objects changes.KOMO discretizes time and represents position derivatives up to order k with discrete equivalents.Source: B. Joint Space Formulation · p. 3
- Joint-space objective terms can use position, velocity, and acceleration, but placement constraints depend on the entire trajectory and couple timesteps.This dependence on prior configurations makes constraint Jacobians complex.Source: B. Joint Space Formulation · p. 3
IV. CARTESIAN FORMULATION
- The Cartesian formulation optimizes the 6-dof pose of a control frame relative to its target frame instead of augmented joint configurations.The relative pose could be observed using 3D pose estimation and robot forward kinematics.Source: §IV · p. 3
- The planner considers key positions and leaves velocity and acceleration control to action-specific controllers.The formulation ignores trajectory velocities and accelerations because dynamic motions may require control frequencies unavailable to this planning stage.Source: §IV · p. 3
- Relative-frame variables make action constraints independent across time, so each constraint depends on a configuration variable at one timestep and has simpler Jacobians.For placing an object, the relative pose between object and table makes the constraint independent of the preceding grasp.Source: §IV · p. 3, Figure 2 · p. 3
- After pick(hook), the end-effector remains attached to the hook during place(hook, shelf), so its world pose is composed through the relative-pose chain.The chain runs from end-effector to hook, shelf, table, and world.Source: Figure 3 · p. 4
- Cross-time dependency moves to the objective, which can measure quantities such as the end-effector’s total travel distance.The constraints instead depend on single-timestep variables.Source: §IV · pp. 3–4
A. Objective Function
The objective combines linear and angular end-effector travel, computed from relative-pose variables. Recovering absolute poses through a changing kinematic tree complicates objective gradients, while the relative formulation simplifies constraint Jacobians.
A. Objective Function
- The objective combines linear and angular end-effector travel between timesteps, with the rotational term using the shortest axis-angle rotation between orientations.Scaling factors α and β weight the two distance components.Source: A. Objective Function · p. 4
- Computing the objective requires mapping relative poses to absolute end-effector poses, complicating its gradient but potentially simplifying constraint Jacobians and action definition.The cross-time dependency is carried by the objective rather than the constraints.Source: A. Objective Function · p. 4
B. Absolute Pose
- A control frame’s world pose is computed by composing relative poses through a kinematic tree whose parent links change when manipulation actions change the frame’s target.After an object is placed, it remains fixed to its target until another action manipulates it.Source: B. Absolute Pose · p. 4
- The absolute-pose objective requires Jacobians with respect to relative-pose variables; their closed-form derivation is nontrivial and provided in Appendix A.The formulation uses the SO(3) exponential map to convert axis-angle orientation to a rotation matrix.Source: B. Absolute Pose · p. 4
V. MANIPULATION PRIMITIVES
The section introduces geometric constraints for manipulation primitives, then specifies how the pick action uses an end-effector control point relative to the target object. The pick constraint places that point inside the object, with its signed distance used to express the condition.
V. MANIPULATION PRIMITIVES
- The primitive constraints are visualized in Figure 4, and the pick distance uses the closest point on the object's mesh to the control point.The Jacobian of the signed squared distance points along the mesh normal at the projection point.Source: §V · p. 4, A. Pick · pp. 4–5
A. Pick
- The pick action takes a movable object as its target, requires the end-effector to hold nothing beforehand, and leaves the object in hand and off other objects.The end-effector is the control frame and the object is the target frame.Source: A. Pick · p. 4
- The pick constraint requires the end-effector control point to lie inside the target object, expressed as a negative signed distance.The lower-level controller is assumed to execute the grasp from a rough desired position on the object.Source: A. Pick · p. 4, Figure 4 · p. 5
B. Place
The place action moves an object from the end-effector onto a target object using touching, support-area, and support-normal constraints. Its support-normal heuristic has a stated geometric limitation.
B. Place
- The place action requires the object to be held and the target to be in the robot's workspace, then leaves the object on the target and no longer in hand.The control frame is the placed object and the target frame is the supporting object.Source: B. Place · p. 5
1) Touch:
- The touch constraint uses the signed distance between the closest points of the two objects, with the angular Jacobian approximated by finite differencing.The contact distance is computed using GJK.Source: 1) Touch · p. 5
2) Support Area:
- The support-area constraint checks whether the placed object's center of mass lies above the target's 2D projection in the plane perpendicular to gravity.For a non-convex placed object, the computation is performed using the center of mass of each convex region.Source: 2) Support Area · p. 5
3) Support Normal:
- The support-normal heuristic constrains the placed object to be above the target's center of mass, assuming the target is convex and relatively flat.For a target such as a sphere, the heuristic may produce a physically infeasible solution; such geometries can be filtered using symbolic preconditions.Source: 3) Support Normal · p. 5
C. Push
The push action uses a held object to move another object into the robot's workspace, with a two-timestep sequence of relative-frame constraints. Its push-normal and push-direction constraints encode contact alignment and target-surface motion.
C. Push
- The push action uses a held object to move another object on a target into the workspace, with one timestep positioning the pusher and a second pushing the object.The pusher is controlled relative to the object being moved in the first timestep, then that object is controlled relative to its target.Source: C. Push · pp. 5–6
1) Push Normal:
- At the first timestep, the push-normal constraint combines touch with alignment of the vector from the contact point to the object's center of mass in the push direction.The linear Jacobian portion assumes the contact point stays fixed to the pusher for small position changes; remaining variables use finite differencing.Source: C. Push · p. 6, 1) Push Normal · p. 6
2) Push Direction:
- At the second timestep, the push-direction constraint restricts the target push position to the target surface, assuming that surface is flat.The three-frame coupling spans at most three timesteps, so the Jacobian depends on at most 18 variables.Source: C. Push · p. 6, 2) Push Direction · p. 6
D. Collision
The section describes collision avoidance and implementation choices for symbolic planning, frame optimization, control, and object tracking. The grasp conversion uses a heuristic stated to be sufficient for the simple objects in the application.
D. Collision
- Each manipulation action includes a collision constraint that keeps manipulated objects separated from environment objects, with its Jacobian computed numerically.The manipulated set comprises objects between the end-effector and control frame; the environment set comprises the remaining objects.Source: D. Collision · p. 6
A. Planning
- The STRIPS planner uses breadth-first tree search for action skeletons, while frame optimization uses IPOPT or NLOPT; Table I compares their scores and times.The selected NLOPT method is Augmented Lagrangian with Sequential Quadratic Programming.Source: A. Planning · p. 6, Table I · p. 7
B. Control
- The pick, place, and push actions use torque-controlled operational-space control with attractive fields toward targets and repulsive fields around obstacles.Torque control is chosen to allow safe interaction with large perception uncertainty.Source: B. Control · p. 6
- The grasp heuristic converts an optimized position inside the object into a top-down two-fingered gripper orientation, but may fail when that position is not graspable.The authors state it is sufficient for the simple objects in their application.Source: B. Control · p. 6
C. Perception
- Object poses are tracked with DBOT particle filtering on downsampled Kinect v2 depth data, with ArUco markers biasing particles toward the expected global pose.The markers address the need to initialize poses and the filter's potential to lose objects moving quickly while occluded.Source: C. Perception · p. 6
VII. RESULTS
The evaluation applies the framework to Tower of Hanoi and Workspace Reach, then demonstrates object-relative TAMP on a real robot with moving objects and noisy perception. The conclusion notes limitations in robot-level collision and joint-limit handling, closed-loop replanning, optimization efficiency, and action-definition scalability.
A. Tower of Hanoi
- Relative pose variables remove the long-term coupling between timesteps from the Tower of Hanoi pick-and-place constraints.The three-block task requires a minimum of 14 actions.Source: A. Tower of Hanoi · p. 7
- The middle-plate Tower of Hanoi plan has a lower optimization score and takes 25s, versus 28s for transferring the tower to the left plate.STRIPS planning finds two candidate action skeletons at a maximum search depth of 14.Source: A. Tower of Hanoi · p. 7
B. Workspace Reach
- For Workspace Reach, placing the hook on the table minimizes end-effector travel; the combined IPOPT optimization takes 3.76s.The three candidate action skeletons differ in where the hook is placed after pushing the box.Source: B. Workspace Reach · p. 7
- IPOPT generally produces more optimal results with fewer constraint violations than NLOPT, but can take up to a minute to find a feasible solution, depending on the instance.The optimization is sensitive to initial object poses.Source: B. Workspace Reach · p. 7
VIII. CONCLUSION
- The framework optimizes Cartesian frames relative to target objects and is demonstrated on a real robot with noisy RGB-D perception, moving objects, and external disturbances.The evaluation uses a 7-dof Franka Panda with a Robotiq 2F-85 gripper in simulation and demonstrates Workspace Reach in the real world.Source: §VIII · p. 7, §VII · p. 7
- Cartesian frames make robot-link collisions and joint limits difficult to encode, so the method checks object-level collisions and relies on lower-level planners or controllers.The authors note that this heuristic may be insufficient in extreme cases.Source: §VIII · p. 7
- The optimizer currently produces open-loop plans; future work includes closed-loop replanning, symbolic-state perception, more efficient optimization, and learning action definitions to reduce engineering effort.Closed-loop planning could respond to symbolic changes beyond reactive controllers; learning-based approaches are proposed to address action-definition scalability.Source: §VIII · pp. 7–8
APPENDIX A SE(3) JACOBIANS
The appendix derives closed-form end-effector Jacobians in SE(3) for gradient-based optimization of object poses. Its position Jacobian uses frame ancestry and the most recent manipulation times to identify which optimization variables affect a frame.
APPENDIX A SE(3) JACOBIANS
- The framework needs Jacobians of object poses in SE(3) with respect to optimization variables in se(3) for gradient-based optimization.The appendix gives closed-form end-effector Jacobians that can be generalized to any object in the kinematic tree.Source: Appendix A · p. 9
A. Position Jacobian
- A position-Jacobian block is derived by the chain rule using a frame’s ancestors and their last manipulated timesteps.A variable affects the global pose of a frame when its timestep is among the last manipulated timesteps of that frame or its ancestors.Source: A. Position Jacobian · p. 9
- The position derivative includes the exponential-map Jacobian relating changes in so(3) coordinates to changes in a rotated vector.The appendix gives a closed-form solution and notes an approximation for θ ≈ 0.Source: A. Position Jacobian · p. 9
B. Orientation Jacobian
The orientation Jacobian differentiates an objective based on the angular change between consecutive end-effector orientations. It uses the derivative of a rotation-matrix trace and accounts for forms of the relative rotation that depend on the kinematic tree.
B. Orientation Jacobian
- The angular objective measures the rotation angle between consecutive end-effector orientations, and its derivative is computed through a matrix trace.The passage describes the derivative of the norm of the logarithmic map as the derivative of a matrix trace.Source: B. Orientation Jacobian · p. 9
- Orientation derivatives with respect to position variables are zero because frame orientations do not depend on those variables.Source: B. Orientation Jacobian · p. 9
- The relative rotation takes one of several forms depending on the kinematic tree, and the corresponding Jacobians are derived from those forms.The passage defines A, B, and C as constant matrices in terms of ω and sets X = exp(ω).Source: B. Orientation Jacobian · p. 9
APPENDIX B OPERATIONAL SPACE CONTROLLER
The operational-space controller uses attractive and repulsive potential fields for manipulation actions such as pick, place, and push. The target field uses perceived poses, while obstacle damping is restricted to motion toward the closest obstacle.
APPENDIX B OPERATIONAL SPACE CONTROLLER
- For pick, place, and push, an attractive field draws the control frame toward the target, while repulsive fields address other objects.The fields use operational-space control at the object level.Source: Appendix B · p. 10
- The attractive potential field is computed from perceived poses of the control and target frames.Source: Appendix B · p. 10
- The repulsive field only damps end-effector velocity toward the closest obstacle, allowing the control frame to contact its target.The passage also defines kobs as the damping-strength gain and 1[·] as an indicator function.Source: Appendix B · p. 10
- The desired acceleration supplied to the operational-space controller combines the goal acceleration with the obstacle acceleration.Source: Appendix B · p. 10