Simple quadratic costs on trajectory. More...
Classes | |
struct | ZMPConstraint |
struct | StaticTorqueCost |
struct | PECost |
struct | FootHeightConstraint |
class | CompositeConfig |
struct | IncrementalRB |
struct | O3Helper |
struct | AngVelCost |
struct | StaticObject |
struct | Collision |
class | CollisionChecker |
Each CollisionChecker object has a copy of the world, so for performance, don't make too many copies. More... | |
struct | CollisionEvaluator |
struct | SingleTimestepCollisionEvaluator |
struct | CastCollisionEvaluator |
class | CollisionCost |
class | CollisionConstraint |
class | Configuration |
class | RobotAndDOF |
Stores an OpenRAVE robot and the active degrees of freedom. More... | |
struct | CartPoseErrCalculator |
struct | CartPoseErrorPlotter |
struct | CartVelJacCalculator |
struct | CartVelCalculator |
class | TrajOptProb |
Holds all the data for a trajectory optimization problem so you can modify it programmatically, e.g. More... | |
struct | TrajOptResult |
struct | BasicInfo |
struct | InitInfo |
Initialization info read from json. More... | |
struct | MakesCost |
struct | MakesConstraint |
struct | TermInfo |
When cost or constraint element of JSON doc is read, one of these guys gets constructed to hold the parameters. More... | |
struct | ProblemConstructionInfo |
This object holds all the data that's read from the JSON document. More... | |
struct | PoseCostInfo |
pose error More... | |
struct | JointPosCostInfo |
Joint space position cost. More... | |
struct | CartVelCntInfo |
Motion constraint on link. More... | |
struct | JointVelCostInfo |
Joint-space velocity squared. More... | |
struct | JointVelConstraintInfo |
struct | CollisionCostInfo |
Collision penalty More... | |
struct | JointConstraintInfo |
joint-space position constraint More... | |
struct | TrajPlotter |
class | JointPosCost |
class | JointVelCost |
class | JointAccCost |
class | Plotter |
Interface for objects that know how to plot themselves given solution vector x. More... | |
Typedefs | |
typedef std::map < OpenRAVE::KinBody::LinkPtr, unsigned > | Link2ID |
typedef boost::shared_ptr < IncrementalRB > | IncrementalRBPtr |
typedef boost::shared_ptr < StaticObject > | StaticObjectPtr |
typedef boost::shared_ptr < CollisionChecker > | CollisionCheckerPtr |
typedef std::map< const OR::KinBody::Link *, int > | Link2Int |
typedef boost::shared_ptr < CollisionEvaluator > | CollisionEvaluatorPtr |
typedef boost::shared_ptr < Configuration > | ConfigurationPtr |
typedef boost::shared_ptr < RobotAndDOF > | RobotAndDOFPtr |
typedef BasicArray< Var > | VarArray |
typedef Json::Value | TrajOptRequest |
typedef Json::Value | TrajOptResponse |
typedef boost::shared_ptr < TermInfo > | TermInfoPtr |
typedef boost::shared_ptr < TrajOptProb > | TrajOptProbPtr |
typedef boost::shared_ptr < TrajOptResult > | TrajOptResultPtr |
typedef boost::shared_ptr < TrajPlotter > | TrajPlotterPtr |
typedef BasicArray< AffExpr > | AffArray |
typedef BasicArray< Cnt > | CntArray |
typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > | DblMatrix |
typedef vector< double > | DblVec |
typedef vector< int > | IntVec |
typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > | TrajArray |
typedef boost::shared_ptr < Plotter > | PlotterPtr |
Enumerations | |
enum | CastCollisionType { CCType_None, CCType_Time0, CCType_Time1, CCType_Between } |
enum | CollisionFilterGroups { RobotFilter = 1, KinBodyFilter = 2 } |
enum | TermType { TT_COST, TT_CNT } |
Functions | |
TRAJOPT_API Eigen::Matrix3d | toMatrix3d (const OpenRAVE::TransformMatrix &tm) |
TRAJOPT_API rbdmath::SpatialTransform | toSpatialTransform (const OpenRAVE::Transform &T) |
OpenRAVE::Vector | toRave (const Eigen::Vector3d &v) |
TRAJOPT_API OpenRAVE::Transform | toRave (const rbdmath::SpatialTransform &T) |
TRAJOPT_API boost::shared_ptr < rbd::Model > | MakeRBDLModel (OpenRAVE::RobotBasePtr robot, bool floating_base, Link2ID &link2id) |
template<typename T > | |
OpenRAVE::UserDataPtr | GetUserData (const T &env, const std::string &key) |
template<typename T > | |
void | SetUserData (T &env, const std::string &key, OpenRAVE::UserDataPtr val) |
template<typename T > | |
void | RemoveUserData (T &body, const std::string &key) |
template<typename T > | |
void | FiniteDifferences (const BasicArray< T > &x, AffArray &dx, double dt) |
OpenRAVE::Vector | toRaveQuat (const Vector4d &v) |
OpenRAVE::Vector | toRaveVector (const Vector3d &v) |
template<typename T > | |
void | extend (vector< T > &a, const vector< T > &b) |
template<typename T > | |
vector< T > | concat (const vector< T > &a, const vector< T > &b, const vector< T > &c) |
template<typename T > | |
vector< T > | concat (const vector< T > &a, const vector< T > &b, const vector< T > &c, const vector< T > &d) |
template<typename T > | |
vector< T > | concat (const vector< T > &a, const vector< T > &b, const vector< T > &c, const vector< T > &d, const vector< T > &e) |
template<typename S , typename T , typename U > | |
vector< U > | cross1 (const vector< S > &a, const vector< T > &b) |
void | exprInc (AffExprVector &a, const AffExprVector &b) |
void | exprDec (AffExprVector &a, const AffExprVector &b) |
AffExprVector | linearizedCrossProduct (const DblVec &x, const AffExprVector &a, const AffExprVector &b) |
AffExprVector | transformExpr (const OR::Transform &T, const AffExprVector &v) |
AffExprVector | rotateExpr (const OR::Vector &rot, const AffExprVector &v) |
MatrixXd | getW (const MatrixXd &qs, double dt) |
TRAJOPT_API std::ostream & | operator<< (std::ostream &, const Collision &) |
CollisionCheckerPtr TRAJOPT_API | CreateCollisionChecker (OR::EnvironmentBaseConstPtr env) |
TRAJOPT_API void | PlotCollisions (const std::vector< Collision > &collisions, OR::EnvironmentBase &env, vector< OR::GraphHandlePtr > &handles, double safe_dist) |
Optimizer::Callback TRAJOPT_API | PlotCallback (TrajOptProb &prob) |
Returns a callback function suitable for an Optimizer. | |
TrajOptProbPtr TRAJOPT_API | ConstructProblem (const ProblemConstructionInfo &) |
TrajOptProbPtr TRAJOPT_API | ConstructProblem (const Json::Value &, OpenRAVE::EnvironmentBasePtr env) |
TrajOptResultPtr TRAJOPT_API | OptimizeProblem (TrajOptProbPtr, bool plot) |
void TRAJOPT_API | SetupPlotting (TrajOptProb &prob, Optimizer &opt) |
OpenRAVE::KinBody::LinkPtr TRAJOPT_API | GetLinkMaybeAttached (OpenRAVE::RobotBasePtr robot, const std::string &name) |
OpenRAVE::RobotBase::ManipulatorPtr TRAJOPT_API | GetManipulatorByName (OpenRAVE::RobotBase &robot, const std::string &name) |
OpenRAVE::RobotBasePtr TRAJOPT_API | GetRobotByName (OpenRAVE::EnvironmentBase &env, const std::string &name) |
OpenRAVE::KinBodyPtr TRAJOPT_API | GetBodyByName (OpenRAVE::EnvironmentBase &env, const std::string &name) |
OpenRAVE::RobotBasePtr TRAJOPT_API | GetRobot (OpenRAVE::EnvironmentBase &env) |
int | GetRobotLinkIndex (const OR::RobotBase &robot, const OR::KinBody::Link &link) |
bool | DoesAffect (const OR::RobotBase &robot, const std::vector< int > &dof_inds, int link_ind) |
void | PlotAxes (OR::EnvironmentBase &env, const OR::Transform &T, float size, std::vector< OR::GraphHandlePtr > &handles) |
TrajArray TRAJOPT_API | getTraj (const DblVec &x, const VarArray &vars) |
Extract trajectory array from solution vector x using indices in array vars. | |
TrajArray TRAJOPT_API | getTraj (const DblVec &x, const AffArray &arr) |
Vector3d | toVector3d (const OR::Vector &v) |
Vector4d | toVector4d (const OR::Vector &v) |
Eigen::Matrix3d | toRot (const OR::Vector &rq) |
OR::Transform | toRaveTransform (const Vector4d &q, const Vector3d &p) |
DblVec | trajToDblVec (const TrajArray &x) |
VectorXd | concat (const VectorXd &a, const VectorXd &b) |
template<typename T > | |
vector< T > | concat (const vector< T > &a, const vector< T > &b) |
template<typename T > | |
vector< T > | singleton (const T &x) |
void TRAJOPT_API | AddVarArrays (OptProb &prob, int rows, const vector< int > &cols, const vector< string > &name_prefix, const vector< VarArray * > &newvars) |
void TRAJOPT_API | AddVarArray (OptProb &prob, int rows, int cols, const string &name_prefix, VarArray &newvars) |
Simple quadratic costs on trajectory.
Optimizer::Callback TRAJOPT_API trajopt::PlotCallback | ( | TrajOptProb & | prob | ) |
Returns a callback function suitable for an Optimizer.
This callback will plot the trajectory (with translucent copies of the robot) as well as all of the Cost and Constraint functions with plot methods