Aikido
|
#include <aikido/robot/ConcreteRobot.hpp>
Public Member Functions | |
ConcreteRobot (const std::string &name, dart::dynamics::MetaSkeletonPtr metaSkeleton, bool simulation, aikido::common::UniqueRNGPtr rng, aikido::control::TrajectoryExecutorPtr trajectoryExecutor, dart::collision::CollisionDetectorPtr collisionDetector, std::shared_ptr< dart::collision::BodyNodeCollisionFilter > selfCollisionFilter) | |
Constructor. More... | |
virtual | ~ConcreteRobot ()=default |
virtual aikido::trajectory::UniqueSplinePtr | smoothPath (const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const aikido::trajectory::Trajectory *path, const constraint::TestablePtr &constraint) override |
Returns a timed trajectory that can be executed by the robot. More... | |
virtual aikido::trajectory::UniqueSplinePtr | retimePath (const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const aikido::trajectory::Trajectory *path) override |
Returns a timed trajectory that can be executed by the robot. More... | |
virtual std::unique_ptr< aikido::trajectory::Spline > | retimePathWithKunz (const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const aikido::trajectory::Trajectory *path, double maxDeviation, double timestep) override |
Returns a timed trajectory computed with KunzRetimer. More... | |
virtual std::future< void > | executeTrajectory (const trajectory::TrajectoryPtr &trajectory) const override |
Executes a trajectory. More... | |
virtual boost::optional< Eigen::VectorXd > | getNamedConfiguration (const std::string &name) const override |
Returns a named configuration. More... | |
virtual void | setNamedConfigurations (std::unordered_map< std::string, const Eigen::VectorXd > namedConfigurations) override |
Sets the list of named configurations. More... | |
virtual std::string | getName () const override |
virtual dart::dynamics::ConstMetaSkeletonPtr | getMetaSkeleton () const override |
virtual aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr | getStateSpace () const override |
virtual void | setRoot (Robot *robot) override |
Sets the root of this robot. More... | |
virtual void | step (const std::chrono::system_clock::time_point &timepoint) override |
Simulates up to the provided timepoint. More... | |
virtual aikido::constraint::dart::CollisionFreePtr | getSelfCollisionConstraint (const statespace::dart::ConstMetaSkeletonStateSpacePtr &space, const dart::dynamics::MetaSkeletonPtr &metaSkeleton) const override |
Returns self-collision constraint. More... | |
virtual aikido::constraint::TestablePtr | getFullCollisionConstraint (const statespace::dart::ConstMetaSkeletonStateSpacePtr &space, const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const constraint::dart::CollisionFreePtr &collisionFree) const override |
TODO: Consider changing this to return a CollisionFreePtr. More... | |
std::shared_ptr< aikido::planner::TrajectoryPostProcessor > | getTrajectoryPostProcessor (const dart::dynamics::MetaSkeletonPtr &metaSkeleton, bool enableShortcut, bool enableBlend, double shortcutTimelimit, double blendRadius, int blendIterations, double feasibilityCheckResolution, double feasibilityApproxTolerance) const |
aikido::trajectory::TrajectoryPtr | planToConfiguration (const aikido::statespace::dart::MetaSkeletonStateSpacePtr &stateSpace, const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const aikido::statespace::StateSpace::State *goalState, const aikido::constraint::dart::CollisionFreePtr &collisionFree, double timelimit) |
TODO: Replace this with Problem interface. More... | |
aikido::trajectory::TrajectoryPtr | planToConfiguration (const aikido::statespace::dart::MetaSkeletonStateSpacePtr &stateSpace, const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const Eigen::VectorXd &goal, const aikido::constraint::dart::CollisionFreePtr &collisionFree, double timelimit) |
TODO: Replace this with Problem interface. More... | |
aikido::trajectory::TrajectoryPtr | planToConfigurations (const aikido::statespace::dart::MetaSkeletonStateSpacePtr &stateSpace, const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const std::vector< aikido::statespace::StateSpace::State * > &goalStates, const aikido::constraint::dart::CollisionFreePtr &collisionFree, double timelimit) |
TODO: Replace this with Problem interface. More... | |
aikido::trajectory::TrajectoryPtr | planToConfigurations (const aikido::statespace::dart::MetaSkeletonStateSpacePtr &stateSpace, const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const std::vector< Eigen::VectorXd > &goals, const aikido::constraint::dart::CollisionFreePtr &collisionFree, double timelimit) |
TODO: Replace this with Problem interface. More... | |
aikido::trajectory::TrajectoryPtr | planToTSR (const aikido::statespace::dart::MetaSkeletonStateSpacePtr &stateSpace, const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const dart::dynamics::BodyNodePtr &bodyNode, const aikido::constraint::dart::TSRPtr &tsr, const aikido::constraint::dart::CollisionFreePtr &collisionFree, double timelimit, std::size_t maxNumTrials, const distance::ConstConfigurationRankerPtr &ranker=nullptr) |
TODO: Replace this with Problem interface. More... | |
aikido::trajectory::TrajectoryPtr | planToTSRwithTrajectoryConstraint (const aikido::statespace::dart::MetaSkeletonStateSpacePtr &stateSpace, const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const dart::dynamics::BodyNodePtr &bodyNode, const aikido::constraint::dart::TSRPtr &goalTsr, const aikido::constraint::dart::TSRPtr &constraintTsr, const aikido::constraint::dart::CollisionFreePtr &collisionFree, double timelimit) |
TODO: Replace this with Problem interface. More... | |
aikido::trajectory::TrajectoryPtr | planToNamedConfiguration (const std::string &name, const aikido::constraint::dart::CollisionFreePtr &collisionFree, double timelimit) |
Plans to a named configuration. More... | |
void | setCRRTPlannerParameters (const util::CRRTPlannerParameters &crrtParameters) |
TODO: This should be revisited once we have Planner API. More... | |
Eigen::VectorXd | getVelocityLimits (const dart::dynamics::MetaSkeleton &metaSkeleton) const |
Computes velocity limits from the MetaSkeleton. More... | |
Eigen::VectorXd | getAccelerationLimits (const dart::dynamics::MetaSkeleton &metaSkeleton) const |
Computes acceleration limits from the MetaSkeleton. More... | |
virtual dart::dynamics::ConstMetaSkeletonPtr | getMetaSkeleton () const=0 |
dart::dynamics::MetaSkeletonPtr | getMetaSkeleton () |
virtual aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr | getStateSpace () const=0 |
aikido::statespace::dart::MetaSkeletonStateSpacePtr | getStateSpace () |
![]() | |
virtual | ~Robot ()=default |
dart::dynamics::MetaSkeletonPtr | getMetaSkeleton () |
aikido::statespace::dart::MetaSkeletonStateSpacePtr | getStateSpace () |
Private Types | |
using | ConfigurationMap = std::unordered_map< std::string, const Eigen::VectorXd > |
Private Member Functions | |
std::unique_ptr< aikido::common::RNG > | cloneRNG () |
Private Attributes | |
Robot * | mRootRobot |
If this robot belongs to another (Composite)Robot, mRootRobot is the topmost robot containing this robot. More... | |
std::string | mName |
Name of this robot. More... | |
dart::dynamics::MetaSkeletonPtr | mMetaSkeleton |
MetaSkeleton of this robot. More... | |
statespace::dart::MetaSkeletonStateSpacePtr | mStateSpace |
dart::dynamics::SkeletonPtr | mParentSkeleton |
Skeleton containing mRobot. More... | |
std::unique_ptr< common::RNG > | mRng |
True if running in simulation mode. More... | |
std::shared_ptr< control::TrajectoryExecutor > | mTrajectoryExecutor |
ConfigurationMap | mNamedConfigurations |
Commonly used configurations. More... | |
::dart::collision::CollisionDetectorPtr | mCollisionDetector |
std::shared_ptr< dart::collision::BodyNodeCollisionFilter > | mSelfCollisionFilter |
util::CRRTPlannerParameters | mCRRTParameters |
|
private |
aikido::robot::ConcreteRobot::ConcreteRobot | ( | const std::string & | name, |
dart::dynamics::MetaSkeletonPtr | metaSkeleton, | ||
bool | simulation, | ||
aikido::common::UniqueRNGPtr | rng, | ||
aikido::control::TrajectoryExecutorPtr | trajectoryExecutor, | ||
dart::collision::CollisionDetectorPtr | collisionDetector, | ||
std::shared_ptr< dart::collision::BodyNodeCollisionFilter > | selfCollisionFilter | ||
) |
Constructor.
[in] | name | Name of the robot. |
[in] | metaSkeleton | Metaskeleton of the robot. |
[in] | simulation | True for running in simulation. |
[in] | rng | Random number generator. |
[in] | trajectoryExecutor | Trajectory executor for the metaSkeleton. |
[in] | collisionDetector | Collision detector. |
[in] | selfCollisionFilter | Collision filter for self collision. |
|
virtualdefault |
|
private |
|
overridevirtual |
Executes a trajectory.
[in] | trajectory | Timed trajectory to execute |
Implements aikido::robot::Robot.
Eigen::VectorXd aikido::robot::ConcreteRobot::getAccelerationLimits | ( | const dart::dynamics::MetaSkeleton & | metaSkeleton | ) | const |
Computes acceleration limits from the MetaSkeleton.
These should be interpreted as absolute in both the positive and negative directions.
[in] | metaSkeleton | MetaSkeleton to compute limits for. |
|
overridevirtual |
TODO: Consider changing this to return a CollisionFreePtr.
Combines provided collision constraint with self collision constraint.
[in] | space | Space in which this collision constraint operates. |
[in] | metaSkeleton | Metaskeleton for the statespace. |
[in] | collisionFree | Collision constraint. |
Implements aikido::robot::Robot.
dart::dynamics::MetaSkeletonPtr aikido::robot::Robot::getMetaSkeleton |
|
overridevirtual |
Implements aikido::robot::Robot.
virtual dart::dynamics::ConstMetaSkeletonPtr aikido::robot::Robot::getMetaSkeleton |
|
overridevirtual |
Implements aikido::robot::Robot.
|
overridevirtual |
Returns a named configuration.
[in] | name | Name of the configuration |
Implements aikido::robot::Robot.
|
overridevirtual |
Returns self-collision constraint.
[in] | space | Space in which this collision constraint operates. |
[in] | metaSkeleton | Metaskeleton for the statespace. |
Implements aikido::robot::Robot.
aikido::statespace::dart::MetaSkeletonStateSpacePtr aikido::robot::Robot::getStateSpace |
|
overridevirtual |
Implements aikido::robot::Robot.
virtual aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr aikido::robot::Robot::getStateSpace |
std::shared_ptr<aikido::planner::TrajectoryPostProcessor> aikido::robot::ConcreteRobot::getTrajectoryPostProcessor | ( | const dart::dynamics::MetaSkeletonPtr & | metaSkeleton, |
bool | enableShortcut, | ||
bool | enableBlend, | ||
double | shortcutTimelimit, | ||
double | blendRadius, | ||
int | blendIterations, | ||
double | feasibilityCheckResolution, | ||
double | feasibilityApproxTolerance | ||
) | const |
[in] | metaSkeleton | The MetaSkeleton whose limits must be respected. |
Eigen::VectorXd aikido::robot::ConcreteRobot::getVelocityLimits | ( | const dart::dynamics::MetaSkeleton & | metaSkeleton | ) | const |
Computes velocity limits from the MetaSkeleton.
These should be interpreted as absolute in both the positive and negative directions.
[in] | metaSkeleton | MetaSkeleton to compute limits for. |
aikido::trajectory::TrajectoryPtr aikido::robot::ConcreteRobot::planToConfiguration | ( | const aikido::statespace::dart::MetaSkeletonStateSpacePtr & | stateSpace, |
const dart::dynamics::MetaSkeletonPtr & | metaSkeleton, | ||
const aikido::statespace::StateSpace::State * | goalState, | ||
const aikido::constraint::dart::CollisionFreePtr & | collisionFree, | ||
double | timelimit | ||
) |
TODO: Replace this with Problem interface.
Plan the robot to a specific configuration. Restores the robot to its initial configuration after planning.
[in] | stateSpace | The StateSpace for the metaskeleton |
[in] | metaSkeleton | Metaskeleton to plan with. |
[in] | goalState | Goal state |
[in] | collisionFree | Testable constraint to check for collision. |
[in] | timelimit | Max time to spend per planning to each IK |
aikido::trajectory::TrajectoryPtr aikido::robot::ConcreteRobot::planToConfiguration | ( | const aikido::statespace::dart::MetaSkeletonStateSpacePtr & | stateSpace, |
const dart::dynamics::MetaSkeletonPtr & | metaSkeleton, | ||
const Eigen::VectorXd & | goal, | ||
const aikido::constraint::dart::CollisionFreePtr & | collisionFree, | ||
double | timelimit | ||
) |
TODO: Replace this with Problem interface.
Wrapper for planToConfiguration using Eigen vectors. Plan the robot to a specific configuration. Restores the robot to its initial configuration after planning.
[in] | stateSpace | The StateSpace for the metaskeleton |
[in] | metaSkeleton | Metaskeleton to plan with. |
[in] | goal | Goal position |
[in] | collisionFree | Testable constraint to check for collision. |
[in] | timelimit | Max time to spend per planning to each IK |
aikido::trajectory::TrajectoryPtr aikido::robot::ConcreteRobot::planToConfigurations | ( | const aikido::statespace::dart::MetaSkeletonStateSpacePtr & | stateSpace, |
const dart::dynamics::MetaSkeletonPtr & | metaSkeleton, | ||
const std::vector< aikido::statespace::StateSpace::State * > & | goalStates, | ||
const aikido::constraint::dart::CollisionFreePtr & | collisionFree, | ||
double | timelimit | ||
) |
TODO: Replace this with Problem interface.
Plan the robot to a set of configurations. Restores the robot to its initial configuration after planning.
[in] | stateSpace | The StateSpace for the metaskeleton |
[in] | metaSkeleton | Metaskeleton to plan with. |
[in] | goalStates | A set of goal states. |
[in] | collisionFree | Testable constraint to check for collision. |
[in] | timelimit | Max time to spend per planning to each IK |
aikido::trajectory::TrajectoryPtr aikido::robot::ConcreteRobot::planToConfigurations | ( | const aikido::statespace::dart::MetaSkeletonStateSpacePtr & | stateSpace, |
const dart::dynamics::MetaSkeletonPtr & | metaSkeleton, | ||
const std::vector< Eigen::VectorXd > & | goals, | ||
const aikido::constraint::dart::CollisionFreePtr & | collisionFree, | ||
double | timelimit | ||
) |
TODO: Replace this with Problem interface.
Plan the robot to a set of configurations. Restores the robot to its initial configuration after planning.
[in] | stateSpace | The StateSpace for the metaskeleton |
[in] | metaSkeleton | Metaskeleton to plan with. |
[in] | goals | A set of goals. |
[in] | collisionFree | Testable constraint to check for collision. |
[in] | timelimit | Max time to spend per planning to each IK |
aikido::trajectory::TrajectoryPtr aikido::robot::ConcreteRobot::planToNamedConfiguration | ( | const std::string & | name, |
const aikido::constraint::dart::CollisionFreePtr & | collisionFree, | ||
double | timelimit | ||
) |
Plans to a named configuration.
[in] | name | Name of the configuration to plan to |
[in] | collisionFree | Collision constraint |
[in] | timelimit | Max time (seconds) to spend |
aikido::trajectory::TrajectoryPtr aikido::robot::ConcreteRobot::planToTSR | ( | const aikido::statespace::dart::MetaSkeletonStateSpacePtr & | stateSpace, |
const dart::dynamics::MetaSkeletonPtr & | metaSkeleton, | ||
const dart::dynamics::BodyNodePtr & | bodyNode, | ||
const aikido::constraint::dart::TSRPtr & | tsr, | ||
const aikido::constraint::dart::CollisionFreePtr & | collisionFree, | ||
double | timelimit, | ||
std::size_t | maxNumTrials, | ||
const distance::ConstConfigurationRankerPtr & | ranker = nullptr |
||
) |
TODO: Replace this with Problem interface.
Plans the configuration of the metakeleton such that the specified bodynode is set to a sample in TSR
[in] | stateSpace | The StateSpace for the metaskeleton. |
[in] | metaSkeleton | The MetaSkeleton to plan with. |
[in] | bodyNode | Bodynode whose frame for which TSR is constructed. |
[in] | tsr | TSR |
[in] | collisionFree | Testable constraint to check for collision. |
[in] | timelimit | Max time (seconds) to spend per planning to each IK |
[in] | maxNumTrials | Max numer of trials to plan. |
[in] | ranker | Ranker to rank the sampled configurations. If nullptr, NominalConfigurationRanker is used with the current metaSkeleton pose. |
aikido::trajectory::TrajectoryPtr aikido::robot::ConcreteRobot::planToTSRwithTrajectoryConstraint | ( | const aikido::statespace::dart::MetaSkeletonStateSpacePtr & | stateSpace, |
const dart::dynamics::MetaSkeletonPtr & | metaSkeleton, | ||
const dart::dynamics::BodyNodePtr & | bodyNode, | ||
const aikido::constraint::dart::TSRPtr & | goalTsr, | ||
const aikido::constraint::dart::TSRPtr & | constraintTsr, | ||
const aikido::constraint::dart::CollisionFreePtr & | collisionFree, | ||
double | timelimit | ||
) |
TODO: Replace this with Problem interface.
Returns a Trajectory that moves the configuration of the metakeleton such that the specified bodynode is set to a sample in a goal TSR and the trajectory is constrained to a constraint TSR
[in] | stateSpace | The StateSpace for the metaskeleton |
[in] | metaSkeleton | The MetaSkeleton to plan with. |
[in] | bodyNode | Bodynode whose frame is meant for TSR |
[in] | goalTsr | The goal TSR to move to |
[in] | constraintTsr | The constraint TSR for the trajectory |
[in] | collisionFree | Collision constraint |
[in] | timelimit | Max time (seconds) to spend per planning to each IK |
|
overridevirtual |
Returns a timed trajectory that can be executed by the robot.
[in] | metaSkeleton | Metaskeleton of the path. |
[in] | path | Geometric path to execute. |
Implements aikido::robot::Robot.
|
overridevirtual |
Returns a timed trajectory computed with KunzRetimer.
[in] | metaSkeleton | Metaskeleton of the path. |
[in] | path | Geometric path to execute. |
[in] | maxDeviation | Maximum deviation allowed from original path. |
[in] | timestep | Time step between trajectory points. |
Implements aikido::robot::Robot.
void aikido::robot::ConcreteRobot::setCRRTPlannerParameters | ( | const util::CRRTPlannerParameters & | crrtParameters | ) |
TODO: This should be revisited once we have Planner API.
Sets CRRTPlanner parameters.
[in] | crrtParameters | CRRT planner parameters |
|
overridevirtual |
Sets the list of named configurations.
[in] | namedConfigurations | Map of name, configuration |
Implements aikido::robot::Robot.
|
overridevirtual |
|
overridevirtual |
Returns a timed trajectory that can be executed by the robot.
[in] | metaSkeleton | Metaskeleton of the path. |
[in] | path | Geometric path to execute. |
[in] | constraint | Must be satisfied after postprocessing. Typically collision constraint is passed. |
Implements aikido::robot::Robot.
|
overridevirtual |
Simulates up to the provided timepoint.
[in] | timepoint | Time to simulate to. |
Implements aikido::robot::Robot.
|
private |
|
private |
|
private |
MetaSkeleton of this robot.
|
private |
Name of this robot.
|
private |
Commonly used configurations.
|
private |
Skeleton containing mRobot.
|
private |
True if running in simulation mode.
|
private |
If this robot belongs to another (Composite)Robot, mRootRobot is the topmost robot containing this robot.
|
private |
|
private |
|
private |