SE3RigidBodyPlanning.cpp
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2010, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ioan Sucan */
12 
13 #include "omplapp/apps/SE3RigidBodyPlanning.h"
14 
15 ompl::base::ScopedState<> ompl::app::SE3RigidBodyPlanning::getDefaultStartState() const
16 {
17  base::ScopedState<base::SE3StateSpace> st(getGeometricComponentStateSpace());
18  aiVector3D s = getRobotCenter(0);
19 
20  st->setX(s.x);
21  st->setY(s.y);
22  st->setZ(s.z);
23  st->rotation().setIdentity();
24 
25  return st;
26 }
aiVector3D getRobotCenter(unsigned int robotIndex) const
Get the robot's center (average of all the vertices of all its parts)
Definition of a scoped state.
Definition: ScopedState.h:120