Saba

With Saba collision-free motions for arbitrary kinematic chains (or loosely defined sets of joints) can be planned efficiently. Therefore an instance of a configuration space is used which usually is initialized with a RobotNodeSet that implicitly defines the number of DoF and the extends of the C-Space by the lower and upper joint limits. Further, a setup for collision detection must be defined as a logical set of object sets which should be checked against collisions. These sets may include parts of the robot (e.g. one arm, head and torso of a humanoid robot) and environmental obstacles.

You can obtain further information by looking at the source code of the examples RrtGui and IKRRT.

• BiRrt Planner
  • Collision Detection
  • Configuration Space
  • Planner
  • Visualization
• IK-RRT
  • Planning Scene
  • Collision Detection
  • Configuration Space
  • IK-Solver
  • Planner
  • Post Processing
• GraspRrt Planner

BiRrt Planner

In the following setup, an instance of a robot is assumed to be loaded and stored in robot. Further, RobotNodeSets (RNS) with name LeftArm, RightArmTorsoHead and LeftArmHand must be present. The first RNS is used for planning and implicitly defines the number of joints and hence, the number of dimensions of the C-Space. The other RNS are used for collision detection, where the RNS LeftArmHand covers all RobotNodes of the left arm and the hand and the RNS RightArmTorsoHead covers all static RobotNodes of the robot that should be considered for collision detection.

// load robot
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(filename);


// create environment
float posX=0.0f, posY=-400.0f, posZ=400.0f, sizeX=100.0f, sizeY=300.0f, sizeZ=1000.0f;
VirtualRobot::ObstaclePtr obstacle = VirtualRobot::Obstacle::createBox( sizeX, sizeY, sizeZ );
Eigen::Affine3f tr(Eigen::Translation3f(posX,posY,posZ));
obstacle->setGlobalPose(tr.matrix());

Collision Detection

Collision detection should be performed between the RNS of the left arm (with hand) and the static part of the robot. Additionally collisions with the environment should be considered.

// setup collision detection
VirtualRobot::CDManagerPtr cdm(new VirtualRobot::CDManager());
VirtualRobot::SceneObjectSetPtr robotArm = robot->getSceneObjectSet("LeftArmHand");
VirtualRobot::SceneObjectSetPtr robotStatic = robot->getSceneObjectSet("RightArmTorsoHead");
cdm->addCollisionModel(robotArm);
cdm->addCollisionModel(robotStatic);
cdm->addCollisionModel(obstacle);

Configuration Space

In this example the CSpaceSampled, the main class for sampling-based motion planning, is used. The C-Space setup is performed by passing a RobotNodeSet to the constructor and setting sampling parameters.

VirtualRobot::RobotNodeSetPtr dof = robot->getRobotNodeSet("LeftArm");
Saba::CSpaceSampledPtr cspace.reset(new CSpaceSampled(robot, cdm, dof);
// set sampling parameter (used for extending a C-Space tree,e.g. with RRT-EXTEND)
cspace->setSamplingSize(sampling_extend_stepsize);
// set sampling size for discrete collision detection (used for path sampling)
cspace->setSamplingSizeDCD(sampling_extend_stepsize);

Planner

In this example a BiRRT planner is used. it is initialized with the CSpace and by setting the start and goal configuration. If motion planning was successful, the resulting trajectory can be obtained with the getSolution method. Further, the RRT data structures are retrieved.

Eigen::VectorXf start(dof->getSize());
Eigen::VectorXf goal(dof->getSize());
// set start ang goal
...

Saba::BiRrt rrt.reset(new Saba::BiRrt(cspace));
rrt->setStart(start);
rrt->setGoal(goal);
bool ok = rrt->plan();
if (!ok)
    std::cout << "Planning failed..." << endl;
else {
    Saba::CSpacePathPtr solution = rrt->getSolution();
    Saba::CSpaceTreePtr tree = rrt->getTree();
    Saba::CSpaceTreePtr tree2 = rrt->getTree2();
}

Visualization

In this example, the planning scene together with the resulting trajectory and the workspace visualization of the RRT are stored to a Coin3D's SoSeparator which e.g. can be displayed with the SoQtExaminerViewer. Have a look at the provided examples for details.

SoSeparator *sep = new SoSeparator();
VirtualRobot::SceneObject::VisualizationType colModel = SceneObject::Full;

// display robot
boost::shared_ptr<VirtualRobot::CoinVisualization> v 
   = robot->getVisualization<VirtualRobot::CoinVisualization>(VirtualRobot::SceneObject::Full);
sep->addChild(visualization->getCoinVisualization());

// display obstacle
VirtualRobot::VisualizationNodePtr vObstacle = obstacle->getVisualization();
std::vector<VirtualRobot::VisualizationNodePtr> visus;
visus.push_back(vObstacle);
boost::shared_ptr<VirtualRobot::CoinVisualization> visualizationO(new VirtualRobot::CoinVisualization(visus));
sep->addChild(visualizationO->getCoinVisualization());

// RRT workspace visualization
boost::shared_ptr<Saba::CoinRrtWorkspaceVisualization> w(new Saba::CoinRrtWorkspaceVisualization(robot, cspace, "Left Hand TCP"));
w->addTree(tree);
w->addTree(tree2);
w->addCSpacePath(solution);
w->addConfiguration(start,Saba::CoinRrtWorkspaceVisualization::eGreen,3.0f);
w->addConfiguration(goal,Saba::CoinRrtWorkspaceVisualization::eGreen,3.0f);
sep->addChild(w->getCoinVisualization());

IK-RRT

The IK-RRT approach can be used to plan grasping motions without initially defining a target configuration. Instead an object together with a set of potential grasps is passed to the planner, defining a set of potential targets in workspace. Hence, a set of potential targets in C-Space is implicitly defined by the set of all IK solutions. The IK-RRT planner samples IK solutions during motion planning and uses these samples as seeds for multiple backward search trees until a collision-free motion can be found.

Planning Scene

In the following example, the robot and the obstacles are accessed via a Scene object. The scene is defined similar to this exemplary file:

<Scene name="PlanningScene">

    <Robot name="iCub" initConfig="init">
        <File>/robots/iCub/iCub.xml</File>
        <Configuration name="init">
            <Node name="Left Arm Shoulder2" unit="radian" value="0.6"/>
            <Node name="Left Arm Elbow1" unit="radian" value="1.8"/>
            <Node name="Left Arm Elbow2" unit="radian" value="-0.5"/>
            <Node name="Left Hand Thumb Joint1" unit="radian" value="0.8"/>
            <Node name="Left Hand Thumb Joint2" unit="radian" value="-0.2"/>
            <Node name="Right Arm Shoulder2" unit="radian" value="0.6"/>
            <Node name="Right Arm Elbow1" unit="radian" value="1.8"/>
            <Node name="Right Arm Elbow2" unit="radian" value="-0.5"/>
            <Node name="Right Hand Thumb Joint1" unit="radian" value="0.8"/>
            <Node name="Right Hand Thumb Joint2" unit="radian" value="-0.2"/>
        </Configuration>

        <RobotNodeSet name="Planning" kinematicRoot="iCubRoot" tcp="Left Arm TCP">
            <Node name="Torso1"/>
            <Node name="Torso2"/>
            <Node name="Torso3"/>
            <Node name="Left Arm Shoulder1"/>
            <Node name="Left Arm Shoulder2"/>
            <Node name="Left Arm Shoulder3"/>
            <Node name="Left Arm Elbow1"/>
            <Node name="Left Arm Elbow2"/>
            <Node name="Left Arm Wrist1"/>
            <Node name="Left Arm Wrist2"/>
        </RobotNodeSet>

        <RobotNodeSet name="ColModel Robot Moving">
            <Node name="Left Arm Shoulder3"/>
            <Node name="Left Arm Elbow2"/>
            <Node name="Left Hand Palm"/>
            <Node name="Left Hand Thumb Joint1"/>
            <Node name="Left Hand Thumb Joint2"/>
            <Node name="Left Hand Thumb Joint3"/>
            <Node name="Left Hand Thumb Joint4"/>
            <Node name="Left Hand Index Joint1"/>
            <Node name="Left Hand Index Joint2"/>
            <Node name="Left Hand Index Joint3"/>
            <Node name="Left Hand Index Joint4"/>
            <Node name="Left Hand Middle Joint1"/>
            <Node name="Left Hand Middle Joint2"/>
            <Node name="Left Hand Middle Joint3"/>
            <Node name="Left Hand Middle Joint4"/>
            <Node name="Left Hand Ring Joint1"/>
            <Node name="Left Hand Ring Joint2"/>
            <Node name="Left Hand Ring Joint3"/>
            <Node name="Left Hand Ring Joint4"/>
            <Node name="Left Hand Pinky Joint1"/>
            <Node name="Left Hand Pinky Joint2"/>
            <Node name="Left Hand Pinky Joint3"/>
            <Node name="Left Hand Pinky Joint4"/>
        </RobotNodeSet>

        <RobotNodeSet name="ColModel Robot Static">
            <Node name="LowerBody"/>
            <Node name="Neck3"/>
            <Node name="Left Leg Waist3"/>
            <Node name="Left Leg Knee"/>
            <Node name="Right Leg Waist3"/>
            <Node name="Right Leg Knee"/>
        </RobotNodeSet>
    </Robot>

    <ManipulationObject name="RiceBox">
        <File type="Inventor">objects/riceBox_iCub_gr0.25.xml</File>
        <GlobalPose>
            <Transform>
                <Translation x="-200" y="-200" z="650"/>
                    <rollpitchyaw units="degree" roll="90" pitch="0" yaw="90"/>
                </Transform>
        </GlobalPose>
    </ManipulationObject>

    <SceneObjectSet name="Environment">
        <SceneObject name="RiceBox"/>
    </SceneObjectSet>

</Scene>

The scene can be accessed as follows:

// load scene
VirtualRobot::ScenePtr scene = VirtualRobot::SceneIO::loadScene("PlanningScene.xml");

// get robot
VirtualRobot::RobotPtr robot = scene->getRobot("iCub");

// get environment
VirtualRobot::SceneObjectSetPtr environment = scene->getSceneObjectSet("Environment");

// get target object which should be grasped
VirtualRobot::ManipulationObjectPtr object = scene->getManipulationObject("RiceBox");

// get the end effector
VirtualRobot::EndEffectorPtr eef = robot->getEndEffector("Left Hand");

// get grasps
VirtualRobot::GraspSetPtr graspSet = object->getGraspSet(eef);

Collision Detection

Similar to the example above, collision detection should be performed between the RNS of the left arm (with hand) and the static part of the robot. Additionally collisions with the environment should be considered, whereas the environment may consist of several obstacles.

// setup collision detection
VirtualRobot::CDManagerPtr cdm(new VirtualRobot::CDManager());
VirtualRobot::SceneObjectSetPtr robotArm = robot->getSceneObjectSet("ColModel Robot Moving");
VirtualRobot::SceneObjectSetPtr robotStatic = robot->getSceneObjectSet("ColModel Robot Static");

cdm->addCollisionModel(robotArm);
cdm->addCollisionModel(robotStatic);
cdm->addCollisionModel(environment);

Configuration Space

Again, the CSpaceSampled is used.

VirtualRobot::RobotNodeSetPtr dof = robot->getRobotNodeSet("Planning");
Saba::CSpaceSampledPtr cspace.reset(new CSpaceSampled(robot, cdm, dof);

// set sampling parameter (used for extending a C-Space tree,e.g. with RRT-EXTEND)
cspace->setSamplingSize(sampling_extend_stepsize);

// set sampling size for discrete collision detection (used for path sampling)
cspace->setSamplingSizeDCD(sampling_extend_stepsize);

IK-Solver

The IK-RRT approach needs an IK-solver in order to sample potential target configurations. In this example the GenerikIK solver is used.

VirtualRobot::GenericIKSolverPtr ikSolver(new VirtualRobot::GenericIKSolver(dof));

// optional: use reachability information for quick reachability filtering
ikSolver->setReachabilityCheck(reachSpace);

// setup collision detection for IK solver
ikSolver->collisionDetection(cdm);

// set maximum translational (mm) and rotational (radian) error 
ikSolver->setMaximumError(10.0f,0.1f);

// set gain factor and number of loops 
ikSolver->setupJacobian(0.9f,20);

Planner

The IK-RRT planner relies on a C-Space, an IK-solver and a start configuration. The goal configuration is not explicitly needed, since it is queried during the planning process.

// get start configuration
Eigen::VectorXf start
dof->getJointValues(start);

// setup planner
Saba::GraspIkRrtPtr ikRrt(new Saba::GraspIkRrt(cspace, object, ikSolver, graspSet));
ikRrt->setStart(start);

// start planning
ikRrt->plan();

Post Processing

Since sampling-based approaches are known to produces non-optimal solutions, the results need to be post-processed in order to achieve appealing trajectories. this can be dine with the ShortcutProcessor as shown in the following example.

Saba::CSpacePathPtr solution = ikRrt->getSolution();
Saba::ShortcutProcessorPtr postProcessing(new Saba::ShortcutProcessor(solution, cspace));
Saba::CSpacePathPtr solutionSmoothed = postProcessing->optimize(100);

GraspRrt Planner

The GraspRRT planner can be used to plan grasping motions without specifying any grasps in advance. The planner combines the search for feasible grasping configurations with the search for collision free motions.

// setup collision detection
CDManagerPtr cdm(new CDManager());
cdm->addCollisionModel(colModelRobA);   // arm and hand
cdm->addCollisionModel(colModelRobB);   // other arm, other hand and torso
cdm->addCollisionModel(colModelEnv);    // environment
cdm->addCollisionModel(targetObject);   // target object

// setup grasp quality measurement
GraspQualityMeasureWrenchSpacePtr graspQuality.reset(new GraspQualityMeasureWrenchSpace(targetObject));

// setup cspace and planner
CSpaceSampledPtr cspace(new CSpaceSampled(robot,cdm,rns));
GraspRrtPtr graspRrt(new GraspRrt(cspace,eef,targetObject,graspQuality,colModelEnv));

// plan motion (implictly creates a reachable grasp)
bool planOK = graspRrt->plan();

// create smooth result
solution = graspRrt>getSolution();
ShortcutProcessorPtr postProcessing(new ShortcutProcessor(solution,cspace));
solutionOptimized = postProcessing->optimize(100);

// get planned grasps
std::vector<GraspRrt::GraspInfo, Eigen::aligned_allocator<GraspRrt::GraspInfo> > vStoreGraspInfo;
graspRrt->getGraspInfoResult(vStoreGraspInfo);