Block Stacking

A long horizon manipulation task leveraging cuRobo is implemented as an example in this library. The block stacking task starts with 9 cubes in front of the robot, with the goal being to stack all the cubes into a tower as seen below.

The key componenet in this task is to design a Pick Place Sequence as shown in the graph below, which consists of moving the robot to a grasp pose, followed by closing the gripper, attaching the grasped object to cuRobo’s robot model, moving the object to a location on top of another cube, opening the gripper, and deleting the object from cuRobo’s robot model. Once this sequence is designed and implemented, the block stacking task can be completed by calling this sequence on all the objects one by one. After each placement, cuRobo updates its world reprensentation by reading the USD stage from Isaac Sim.

digraph { edge [color = "#2B4162"; fontsize=10]; node [shape="box", style="rounded, filled", fontsize=12, color="#cccccc"] "Isaac Sim" [label="Isaac Sim", shape="box", color="#cccccc", style="rounded,filled"] world_coll [label="cuRobo WorldCollision",shape="box",color="#76b900", style="filled, rounded", fontcolor="white"] motion_gen [label="cuRobo MotionGen", shape="box",shape="box",color="#76b900", style="filled, rounded", fontcolor="white"] T -> world_coll; {C, G} -> motion_gen; {E, P} -> motion_gen; motion_gen -> world_coll; "Isaac Sim" -> usd_help[label="Usd Stage", style="dashed", fontcolor="#708090"]; "Isaac Sim" -> {C, G, E} [label="Robot State", style="dashed", fontcolor="#708090"]; {C, G} -> "Isaac Sim"[label="Robot Action", style="dashed", fontcolor="#708090"]; H -> "Isaac Sim" [label="Open Gripper", style="dashed", color="#558c8c", fontcolor="#558c8c"]; D -> "Isaac Sim" [label="Close Gripper", style="dashed", color="#558c8c", fontcolor="#558c8c"]; subgraph cluster_pick_place { label="Pick Place Sequence"; A [label="Start", shape="plain", style="rounded"] C [label="Plan and Move", shape="box", color="#76b900", fontcolor="white"] D [label="Object in Grasp",style="rounded", color=black] E [label="Attach Object to Kinematics", shape="box", color="#76b900", fontcolor="white"] G [label="Plan and Move", shape="box", color="#76b900", fontcolor="white"] H [label="Release Object",style="rounded", color=black] P [label="Detach Object from Kinematics", shape="box", color="#76b900", fontcolor="white"] Q [label="End", shape="plain",style="rounded"] A -> C[label="Grasp Pose"]; C -> D[label="Close Gripper", color="#558c8c", fontcolor="#558c8c"]; D -> E; E -> G[label="Placement Pose"]; G -> H[label="Object in \n Placement Pose"]; H -> P; P -> Q; } subgraph cluster_world_monitor { label= "cuRobo World Monitor"; T [label="WorldConfig", shape="box", color="#76b900", fontcolor="white"] usd_help [label="UsdHelper",shape="box", color="#76b900", style="filled, rounded", fontcolor="white"] usd_help -> T; } }

Attaching Object to Robot

cuRobo implements an attach object feature to enable reasoning about collisions between a grasped object and the world while the robot is moving. Since cuRobo represents the robot with spheres, cuRobo automatically computes bounding spheres (see Geometry Approximation to Spheres) for an object to attach to the robot. When attaching the object to the robot, the object is also deleted from the world. Similarly when the object is detached, the spheres are disabled and the object is enabled in the world model. This sequence of actions is shown in the graph below.

digraph { edge [color = "#708090"; fontsize=10]; node [shape="box", style="rounded, filled", fontsize=12, color="#cccccc"] world_coll [label="cuRobo WorldCollision",shape="box",color="#76b900", style="filled, rounded", fontcolor="white"] cu_kin [label="CudaRobotModel",shape="box",color="#76b900", style="filled, rounded", fontcolor="white"] {K,N} -> cu_kin; {L, O} -> world_coll; subgraph cluster_attach_object { label= "Attach Object to Kinematics"; color= "#76b900"; M [label="Generate Spheres for Object"] N [label="Attach Spheres to Kinematics"] O [label="Disable obstacle in world"] M -> N; N -> O; } subgraph cluster_detach_object { label = "Detach Object from Kinematics"; color= "#76b900"; K [label="Remove spheres"] L [label="Enable obstacle in world"] K -> L; } }

Note

Detach object currently deletes all spheres attached to a link.