A Task Space Region (TSR). TSRs are workspace regions that impose a constraints on a robot. They are composed of a pose (in some reference frame) and tolerances on position and rotation. initialize() must be called once all parameters are set. More...

#include <tsr.h>

Classes
class	Specification
	The specification of a TSR. More...

Public Member Functions
Constructor and Initialization
	TSR (const WorldPtr &world, const Specification &spec)
	Constructor. More...

	~TSR ()
	Destructor. Disables IK on node if created. More...

void	setWorld (const WorldPtr &world)
	Set the world used by this TSR. More...

void	clear ()
	Clears the initialization of this TSR. More...

void	initialize ()
	Initialize this TSR. Creates IK node attached to body node in world. More...

DoF Indexing
void	useGroup (const std::string &name)
	Use the joints in a robot's group. Robot used is the target frame's structure. More...

void	useIndices (const std::vector< std::size_t > &indices)
	Use DoF indices for TSR computation. More...

robowflex::RobotPose	getTransformToFrame () const
	Gets the transformation from the specification's base to the TSR's frame. More...

void	useWorldIndices (const std::vector< std::pair< std::size_t, std::size_t >> &indices)
	Use World DoF indices for TSR computation. World indices are pairs of skeleton index and DoF index. More...

void	setWorldIndices (const std::vector< std::pair< std::size_t, std::size_t >> &indices)
	Output world indices. TSR information for *WorldState methods uses this set of world indices. World indices are pairs of skeleton index and DoF index. More...

std::vector< std::pair< std::size_t, std::size_t > >	computeWorldIndices () const
	Compute the set of world indices used based on the underlying active indices. More...

std::size_t	getSkeletonIndex ()
	Get the skeleton index for the target frame's structure. More...

const std::vector< std::size_t > &	getIndices () const
	Get the set of indices used for TSR computation. More...

const std::vector< std::pair< std::size_t, std::size_t > > &	getWorldIndices () const
	Get the set of output world indices used. More...

void	toBijection (Eigen::Ref< Eigen::VectorXd > world, const Eigen::Ref< const Eigen::VectorXd > &state) const
	From a configuration of controlled indices, map that configuration into a world configuration. That is, map state into world according to the set indices and world indices. More...

void	fromBijection (Eigen::Ref< Eigen::VectorXd > state, const Eigen::Ref< const Eigen::VectorXd > &world) const
	From a world configuration, map that configuration into a controlled DoF configuration. That is, map world into state according to the set indices and world indices. More...

Getters
std::size_t	getDimension () const
	Get the error dimension of this TSR. More...

std::size_t	getNumDofs () const
	Get the number of controlled DoF for this TSR. More...

std::size_t	getNumWorldDofs () const
	Get the number of world DoF for this TSR. More...

Specification and Updates
Specification &	getSpecification ()
	Get the specification for this TSR. More...

void	updatePose ()
	If the pose of the specification is updated, call this to update underlying IK solver. More...

void	updateBounds ()
	If the bounds of the specification are updated, call this to update underlying IK solver. More...

void	updateSolver ()
	If the solver parameters of the specification are updated, call this to update underlying IK solver. More...

Error / Function Computation
void	getErrorWorldRaw (Eigen::Ref< Eigen::VectorXd > error) const
	Get the current error given the world state, with all values. More...

void	getErrorWorld (Eigen::Ref< Eigen::VectorXd > error) const
	Get the current error given the world state. More...

void	getErrorWorldState (const Eigen::Ref< const Eigen::VectorXd > &world, Eigen::Ref< Eigen::VectorXd > error) const
	Get the error given a provided world configuration. More...

void	getError (const Eigen::Ref< const Eigen::VectorXd > &state, Eigen::Ref< Eigen::VectorXd > error) const
	Get the error given a configuration of the controlled DoF. More...

Jacobian Computation
void	getJacobianWorld (Eigen::Ref< Eigen::MatrixXd > jacobian) const
	Get the current Jacobian given the world state. More...

void	getJacobianWorldState (const Eigen::Ref< const Eigen::VectorXd > &world, Eigen::Ref< Eigen::MatrixXd > jacobian) const
	Get the Jacobian given a provided world configuration. More...

void	getJacobian (const Eigen::Ref< const Eigen::VectorXd > &state, Eigen::Ref< Eigen::MatrixXd > jacobian) const
	Get the Jacobian given a configuration of the controlled DoF. More...

Distance Computation
double	distanceWorld () const
	Get the current distance to satisfaction given the world state. More...

double	distanceWorldState (const Eigen::Ref< const Eigen::VectorXd > &world) const
	Get the distance to satisfaction given a provided world configuration. More...

double	distance (const Eigen::Ref< const Eigen::VectorXd > &state) const
	Get the distance to satisfaction given a configuration of the controlled DoF. More...

Solving for Satisfying Configurations
bool	solveWorld ()
	Solve for a satisfying configuration given the current state of the world. More...

bool	solveWorldState (Eigen::Ref< Eigen::VectorXd > world)
	Solve for a satisfying configuration given an initial world configuration. More...

bool	solve (Eigen::Ref< Eigen::VectorXd > state)
	Solve for a satisfying configuration given an initial configuration of the controlled DoF. More...

bool	solveGradientWorld ()
	Solve using gradient descent for a satisfying configuration given the current state of the world. More...

bool	solveGradientWorldState (Eigen::Ref< Eigen::VectorXd > world)
	Solve using gradient descent for a satisfying configuration given an initial world configuration. More...

bool	solveGradient (Eigen::Ref< Eigen::VectorXd > state)
	Solve using gradient descent for a satisfying configuration given an initial configuration of the controlled DoF. More...

Getting and Setting Position
void	setPositionsWorldState (const Eigen::Ref< const Eigen::VectorXd > &world) const
	Set the positions of the world given a world configuration. More...

void	setPositions (const Eigen::Ref< const Eigen::VectorXd > &state) const
	Set the positions of the world given a configuration. More...

void	getPositionsWorldState (Eigen::Ref< Eigen::VectorXd > world) const
	Get the positions of the world into a world configuration. More...

void	getPositions (Eigen::Ref< Eigen::VectorXd > state) const
	Get the positions of the world given into a configuration. More...

Private Member Functions
void	computeBijection ()
	Compute the mapping from world indices to controlled DoF indices. More...

Private Attributes
WorldPtr	world_
	Underlying world. More...

Specification	spec_
	TSR specification. More...

std::size_t	skel_index_
	Index of controlled skeleton. More...

std::vector< std::size_t >	indices_
	Controlled indices. More...

std::vector< std::pair< std::size_t, std::size_t > >	world_indices_
	World indices. More...

std::vector< std::size_t >	bijection_

std::shared_ptr< dart::dynamics::SimpleFrame >	frame_ {nullptr}
	Target frame of TSR. More...

dart::dynamics::BodyNode *	tnd_ {nullptr}
	Target body node. More...

std::shared_ptr< dart::dynamics::InverseKinematics >	ik_ {nullptr}
	Inverse kinematics module. More...

dart::dynamics::InverseKinematics::TaskSpaceRegion *	tsr_ {nullptr}
	TSR. More...

Detailed Description

A Task Space Region (TSR). TSRs are workspace regions that impose a constraints on a robot. They are composed of a pose (in some reference frame) and tolerances on position and rotation. initialize() must be called once all parameters are set.

In the TSR, there are controlled DoF (through useIndices()) and world DoF (through useWorldIndices()). The controlled DoF are the DoF inside the structure the TSR's target frame is. These are the DoF that are used as part of the error, Jacobian, and other computations. However, the world might be using more DoF, especially if it is a multi-robot system. The world's DoF are given through useWorldIndices(). The *WorldState allow you to pass a configuration for the entire world (where each entry in that configuration corresponds to the indices given) and have the input / output be mapped appropriately to the controlled DoF.

Note, the TSR creates an IK node in the underlying Dart skeleton for the provided world. If you have multiple TSRs for the same target frame these will overwrite each other, unless you use a clone of the World.

Definition at line 62 of file tsr.h.

Constructor & Destructor Documentation

◆ TSR()

TSR::TSR	(	const WorldPtr &	world,
		const Specification &	spec
	)

Constructor.

Parameters

[in]	world	World to apply TSR to.
[in]	spec	Specification for TSR.

TSR

Definition at line 465 of file tsr.cpp.

                                                          : world_(world), spec_(spec)
 {
 }

◆ ~TSR()

TSR::~TSR ( )

Destructor. Disables IK on node if created.

Definition at line 469 of file tsr.cpp.

 {
     clear();
 }

Member Function Documentation

◆ clear()

void TSR::clear ( void )

Clears the initialization of this TSR.

Definition at line 474 of file tsr.cpp.

 {
     if (tnd_ and ik_)
         tnd_->clearIK();
  
     frame_ = nullptr;
     ik_ = nullptr;
     tnd_ = nullptr;
     tsr_ = nullptr;
 }

◆ computeBijection()

void TSR::computeBijection ( )

private

Compute the mapping from world indices to controlled DoF indices.

Definition at line 897 of file tsr.cpp.

 {
     if (world_indices_.empty())
         return;
  
     bijection_.resize(indices_.size());
  
     bool same = world_indices_.size() == indices_.size();
     for (std::size_t i = 0; i < indices_.size(); ++i)
     {
         bijection_[i] = world_indices_.size();
         for (std::size_t j = 0; j < world_indices_.size(); ++j)
         {
             auto entry = world_indices_[j];
             if (entry.first == getSkeletonIndex() and entry.second == indices_[i])
             {
                 bijection_[i] = j;
                 same &= i == j;
                 break;
             }
         }
     }
  
     if (same)
         bijection_.clear();
 }

◆ computeWorldIndices()

std::vector< std::pair< std::size_t, std::size_t > > TSR::computeWorldIndices ( ) const

Compute the set of world indices used based on the underlying active indices.

Returns: World indices corresponding to this TSR's current indices.

Definition at line 544 of file tsr.cpp.

 {
     std::vector<std::pair<std::size_t, std::size_t>> wi;
     for (const auto &index : indices_)
         wi.emplace_back(skel_index_, index);
  
     return wi;
 }

◆ distance()

double TSR::distance ( const Eigen::Ref< const Eigen::VectorXd > & state ) const

Get the distance to satisfaction given a configuration of the controlled DoF.

Parameters

[in] state Controlled DoF configuration.

Returns: Distance to TSR satisfaction.

Definition at line 685 of file tsr.cpp.

 {
     Eigen::VectorXd x(getDimension());
     getError(state, x);
     return x.norm();
 }

◆ distanceWorld()

double TSR::distanceWorld ( ) const

Get the current distance to satisfaction given the world state.

Returns: Distance to TSR satisfaction.

Definition at line 671 of file tsr.cpp.

 {
     Eigen::VectorXd x(getDimension());
     getErrorWorld(x);
     return x.norm();
 }

◆ distanceWorldState()

double TSR::distanceWorldState ( const Eigen::Ref< const Eigen::VectorXd > & world ) const

Get the distance to satisfaction given a provided world configuration.

Parameters

[in] world World configuration (from world indices).

Returns: Distance to TSR satisfaction.

Definition at line 678 of file tsr.cpp.

 {
     Eigen::VectorXd x(getDimension());
     getErrorWorldState(world, x);
     return x.norm();
 }

◆ fromBijection()

void TSR::fromBijection	(	Eigen::Ref< Eigen::VectorXd >	state,
		const Eigen::Ref< const Eigen::VectorXd > &	world
	)		const

From a world configuration, map that configuration into a controlled DoF configuration. That is, map world into state according to the set indices and world indices.

Parameters

[out]	state	Output configuration.
[in]	world	Input world configuration.

Definition at line 886 of file tsr.cpp.

 {
     if (bijection_.empty())
         state = world;
     else
         for (std::size_t i = 0; i < bijection_.size(); ++i)
             if (bijection_[i] < world_indices_.size())
                 state[i] = world[bijection_[i]];
 }

◆ getDimension()

std::size_t TSR::getDimension ( ) const

Get the error dimension of this TSR.

Returns: The error dimension of the TSR.

Definition at line 558 of file tsr.cpp.

 {
     return spec_.dimension;
 }

◆ getError()

void TSR::getError	(	const Eigen::Ref< const Eigen::VectorXd > &	state,
		Eigen::Ref< Eigen::VectorXd >	error
	)		const

Get the error given a configuration of the controlled DoF.

Parameters

[in]	state	Controlled DoF configuration.
[out]	error	Error value.

Definition at line 621 of file tsr.cpp.

 {
     setPositions(state);
     getErrorWorld(error);
 }

◆ getErrorWorld()

void TSR::getErrorWorld ( Eigen::Ref< Eigen::VectorXd > error ) const

Get the current error given the world state.

Parameters

[out] error Error value.

Definition at line 592 of file tsr.cpp.

 {
     world_->lock();
  
     auto tsr_error = tsr_->computeError();
  
     std::size_t j = 0;
     for (std::size_t i = 0; i < 6; ++i)
     {
         if (spec_.indices[i])
             error[j++] = tsr_error[i];
     }
  
     world_->unlock();
 }

◆ getErrorWorldRaw()

void TSR::getErrorWorldRaw ( Eigen::Ref< Eigen::VectorXd > error ) const

Get the current error given the world state, with all values.

Parameters

[out] error Error value for all TSRs.

Definition at line 585 of file tsr.cpp.

 {
     world_->lock();
     error = tsr_->computeError();
     world_->unlock();
 }

◆ getErrorWorldState()

void TSR::getErrorWorldState	(	const Eigen::Ref< const Eigen::VectorXd > &	world,
		Eigen::Ref< Eigen::VectorXd >	error
	)		const

Get the error given a provided world configuration.

Parameters

[in]	world	World configuration (from world indices).
[out]	error	Error value.

Definition at line 608 of file tsr.cpp.

 {
     if (bijection_.empty())
         getError(world, error);
     else
     {
         Eigen::VectorXd state(getNumDofs());
         fromBijection(state, world);
         getError(state, error);
     }
 }

◆ getIndices()

const std::vector< std::size_t > & TSR::getIndices ( ) const

Get the set of indices used for TSR computation.

Definition at line 539 of file tsr.cpp.

 {
     return indices_;
 }

◆ getJacobian()

void TSR::getJacobian	(	const Eigen::Ref< const Eigen::VectorXd > &	state,
		Eigen::Ref< Eigen::MatrixXd >	jacobian
	)		const

Get the Jacobian given a configuration of the controlled DoF.

Parameters

[in]	state	Controlled DoF configuration.
[out]	jacobian	Jacobian value.

Definition at line 662 of file tsr.cpp.

 {
     world_->lock();
     setPositions(state);
     getJacobianWorld(jacobian);
     world_->unlock();
 }

◆ getJacobianWorld()

void TSR::getJacobianWorld ( Eigen::Ref< Eigen::MatrixXd > jacobian ) const

Get the current Jacobian given the world state.

Parameters

[out] jacobian Jacobian value.

Definition at line 627 of file tsr.cpp.

 {
     world_->lock();
  
     auto tjac = ik_->computeJacobian();
  
     std::size_t j = 0;
     for (std::size_t i = 0; i < 6; ++i)
     {
         if (spec_.indices[i])
             jacobian.row(j++) = tjac.row(i);
     }
  
     world_->unlock();
 }

◆ getJacobianWorldState()

void TSR::getJacobianWorldState	(	const Eigen::Ref< const Eigen::VectorXd > &	world,
		Eigen::Ref< Eigen::MatrixXd >	jacobian
	)		const

Get the Jacobian given a provided world configuration.

Parameters

[in]	world	World configuration (from world indices).
[out]	jacobian	Jacobian value.

Definition at line 643 of file tsr.cpp.

 {
     if (bijection_.empty())
         getJacobian(world, jacobian);
     else
     {
         Eigen::VectorXd state(getNumDofs());
         fromBijection(state, world);
  
         Eigen::VectorXd tjac(getDimension(), getNumDofs());
         getJacobian(state, tjac);
  
         for (std::size_t i = 0; i < bijection_.size(); ++i)
             if (bijection_[i] < world_indices_.size())
                 jacobian.col(bijection_[i]) = tjac.col(i);
     }
 }

◆ getNumDofs()

std::size_t TSR::getNumDofs ( ) const

Get the number of controlled DoF for this TSR.

Returns: The number of DoF for this TSR.

Definition at line 563 of file tsr.cpp.

 {
     return indices_.size();
 }

◆ getNumWorldDofs()

std::size_t TSR::getNumWorldDofs ( ) const

Get the number of world DoF for this TSR.

Returns: The number of world DoF for this TSR.

Definition at line 568 of file tsr.cpp.

 {
     return world_indices_.size();
 }

◆ getPositions()

void TSR::getPositions ( Eigen::Ref< Eigen::VectorXd > state ) const

Get the positions of the world given into a configuration.

Parameters

[out] state Controlled DoF configuration.

Definition at line 803 of file tsr.cpp.

 {
     world_->lock();
     state = ik_->getPositions();
     world_->unlock();
 }

◆ getPositionsWorldState()

void TSR::getPositionsWorldState ( Eigen::Ref< Eigen::VectorXd > world ) const

Get the positions of the world into a world configuration.

Parameters

[out] world World configuration.

Definition at line 796 of file tsr.cpp.

 {
     Eigen::VectorXd state(getNumDofs());
     getPositions(state);
     toBijection(world, state);
 }

◆ getSkeletonIndex()

std::size_t TSR::getSkeletonIndex ( )

Get the skeleton index for the target frame's structure.

Returns: The index of the skeleton this TSR is targeting.

Definition at line 531 of file tsr.cpp.

 {
     if (not tnd_)
         initialize();
  
     return skel_index_;
 }

◆ getSpecification()

TSR::Specification & TSR::getSpecification ( )

Get the specification for this TSR.

Returns: The TSR's specification.

Definition at line 810 of file tsr.cpp.

 {
     return spec_;
 }

◆ getTransformToFrame()

robowflex::RobotPose TSR::getTransformToFrame ( ) const

Gets the transformation from the specification's base to the TSR's frame.

Returns: RobotPose representing the transform.

Definition at line 573 of file tsr.cpp.

 {
     const auto &sim = world_->getSim();
     const auto &bskl = sim->getSkeleton(spec_.base.structure);
     auto *bnd = bskl->getBodyNode(spec_.base.frame);
  
     if (not tnd_)
         throw std::runtime_error("Target body node is not initialized");
  
     return tnd_->getTransform(bnd);
 }

◆ getWorldIndices()

const std::vector< std::pair< std::size_t, std::size_t > > & TSR::getWorldIndices ( ) const

Get the set of output world indices used.

Definition at line 553 of file tsr.cpp.

 {
     return world_indices_;
 }

◆ initialize()

void TSR::initialize ( )

Initialize this TSR. Creates IK node attached to body node in world.

Definition at line 839 of file tsr.cpp.

 {
     const auto &sim = world_->getSim();
     const auto &tskl = sim->getSkeleton(spec_.target.structure);
     if (not tskl)
         throw std::runtime_error("Target skeleton " + spec_.target.structure + " in TSR does not exist!");
     skel_index_ = world_->getSkeletonIndex(tskl);
  
     const auto &bskl = sim->getSkeleton(spec_.base.structure);
     if (not bskl)
         throw std::runtime_error("Base skeleton " + spec_.base.structure + " in TSR does not exist!");
  
     tnd_ = tskl->getBodyNode(spec_.target.frame);
     ik_ = tnd_->getIK(true);
     frame_ = ik_->getTarget();
  
     if (spec_.base.frame != magic::ROOT_FRAME)
     {
         auto *bnd = bskl->getBodyNode(spec_.base.frame);
         frame_ = frame_->clone(bnd);
     }
  
     ik_->setTarget(frame_);
  
     tsr_ = &ik_->setErrorMethod<dart::dynamics::InverseKinematics::TaskSpaceRegion>();
     tsr_->setComputeFromCenter(false);
  
     updatePose();
     updateBounds();
     updateSolver();
  
     if (indices_.empty())
         indices_ = ik_->getDofs();
     else
         ik_->setDofs(indices_);
 }

◆ setPositions()

void TSR::setPositions ( const Eigen::Ref< const Eigen::VectorXd > & state ) const

Set the positions of the world given a configuration.

Parameters

[in] state Controlled DoF configuration.

Definition at line 789 of file tsr.cpp.

 {
     world_->lock();
     ik_->setPositions(state);
     world_->unlock();
 }

◆ setPositionsWorldState()

void TSR::setPositionsWorldState ( const Eigen::Ref< const Eigen::VectorXd > & world ) const

Set the positions of the world given a world configuration.

Parameters

[in] world World configuration.

Definition at line 782 of file tsr.cpp.

 {
     Eigen::VectorXd state(getNumDofs());
     fromBijection(state, world);
     setPositions(state);
 }

◆ setWorld()

void TSR::setWorld ( const WorldPtr & world )

Set the world used by this TSR.

Parameters

[in] world New world to use.

Definition at line 485 of file tsr.cpp.

 {
     clear();
     world_ = world;
     initialize();
 }

◆ setWorldIndices()

void TSR::setWorldIndices ( const std::vector< std::pair< std::size_t, std::size_t >> & indices )

Output world indices. TSR information for *WorldState methods uses this set of world indices. World indices are pairs of skeleton index and DoF index.

Parameters

[in] indices Indices to use.

Definition at line 525 of file tsr.cpp.

 {
     world_indices_ = indices;
     computeBijection();
 }

◆ solve()

bool TSR::solve ( Eigen::Ref< Eigen::VectorXd > state )

Solve for a satisfying configuration given an initial configuration of the controlled DoF.

Parameters

[in] state Controlled DoF configuration.

Returns: True on success, false on failure.

Definition at line 718 of file tsr.cpp.

 {
     world_->lock();
  
     setPositions(state);
     bool r = solveWorld();
     getPositions(state);
  
     world_->unlock();
     return r;
 }

◆ solveGradient()

bool TSR::solveGradient ( Eigen::Ref< Eigen::VectorXd > state )

Solve using gradient descent for a satisfying configuration given an initial configuration of the controlled DoF.

Parameters

[in] state Controlled DoF configuration.

Returns: True on success, false on failure.

Definition at line 754 of file tsr.cpp.

 {
     world_->lock();
  
     unsigned int iter = 0;
     double norm = 0;
     Eigen::VectorXd f(getDimension());
     Eigen::MatrixXd j(getDimension(), getNumDofs());
  
     const double squared_tolerance = spec_.tolerance * spec_.tolerance;
  
     setPositions(state);
     getErrorWorld(f);
  
     while ((norm = f.norm()) > squared_tolerance && iter++ < spec_.maxIter)
     {
         getJacobianWorld(j);
         state -= j.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(f);
  
         setPositions(state);
         getErrorWorld(f);
     }
  
     world_->unlock();
  
     return norm < squared_tolerance;
 }

◆ solveGradientWorld()

bool TSR::solveGradientWorld ( )

Solve using gradient descent for a satisfying configuration given the current state of the world.

Returns: True on success, false on failure.

Definition at line 730 of file tsr.cpp.

 {
     world_->lock();
  
     Eigen::VectorXd state = ik_->getPositions();
     bool r = solveGradient(state);
     setPositions(state);
  
     world_->unlock();
     return r;
 }

◆ solveGradientWorldState()

bool TSR::solveGradientWorldState ( Eigen::Ref< Eigen::VectorXd > world )

Solve using gradient descent for a satisfying configuration given an initial world configuration.

Parameters

[in] world World configuration (from world indices).

Returns: True on success, false on failure.

Definition at line 742 of file tsr.cpp.

 {
     if (bijection_.empty())
         return solveGradient(world);
  
     Eigen::VectorXd state(getNumDofs());
     fromBijection(state, world);
     bool r = solveGradient(state);
     toBijection(world, state);
     return r;
 }

◆ solveWorld()

bool TSR::solveWorld ( )

Solve for a satisfying configuration given the current state of the world.

Returns: True on success, false on failure.

Definition at line 692 of file tsr.cpp.

 {
     if (not ik_)
     {
         RBX_ERROR("TSR: Solve called before initialize!");
         return false;
     }
  
     world_->lock();
     bool r = ik_->solveAndApply();
     world_->unlock();
     return r;
 }

◆ solveWorldState()

bool TSR::solveWorldState ( Eigen::Ref< Eigen::VectorXd > world )

Solve for a satisfying configuration given an initial world configuration.

Parameters

[in] world World configuration (from world indices).

Returns: True on success, false on failure.

Definition at line 706 of file tsr.cpp.

 {
     if (bijection_.empty())
         return solve(world);
  
     Eigen::VectorXd state(getNumDofs());
     fromBijection(state, world);
     bool r = solve(state);
     toBijection(world, state);
     return r;
 }

◆ toBijection()

void TSR::toBijection	(	Eigen::Ref< Eigen::VectorXd >	world,
		const Eigen::Ref< const Eigen::VectorXd > &	state
	)		const

From a configuration of controlled indices, map that configuration into a world configuration. That is, map state into world according to the set indices and world indices.

Parameters

[out]	world	Output world configuration.
[in]	state	Input configuration.

Definition at line 876 of file tsr.cpp.

 {
     if (bijection_.empty())
         world = state;
     else
         for (std::size_t i = 0; i < bijection_.size(); ++i)
             if (bijection_[i] < world_indices_.size())
                 world[bijection_[i]] = state[i];
 }

◆ updateBounds()

void TSR::updateBounds ( )

If the bounds of the specification are updated, call this to update underlying IK solver.

Definition at line 821 of file tsr.cpp.

 {
     if (tsr_)
     {
         tsr_->setLinearBounds(spec_.position.lower, spec_.position.upper);
         tsr_->setAngularBounds(spec_.orientation.lower, spec_.orientation.upper);
     }
 }

◆ updatePose()

void TSR::updatePose ( )

If the pose of the specification is updated, call this to update underlying IK solver.

Definition at line 815 of file tsr.cpp.

 {
     if (frame_)
         frame_->setRelativeTransform(spec_.pose);
 }

◆ updateSolver()

void TSR::updateSolver ( )

If the solver parameters of the specification are updated, call this to update underlying IK solver.

Definition at line 830 of file tsr.cpp.

 {
     if (ik_)
     {
         ik_->getSolver()->setTolerance(spec_.tolerance);
         ik_->getSolver()->setNumMaxIterations(spec_.maxIter);
     }
 }

◆ useGroup()

void TSR::useGroup ( const std::string & name )

Use the joints in a robot's group. Robot used is the target frame's structure.

Parameters

[in] name Name of group to use.

Definition at line 492 of file tsr.cpp.

 {
     auto robot = world_->getRobot(spec_.target.structure);
     if (not robot)
         throw std::runtime_error("Target robot does exist in world!");
  
     useIndices(robot->getGroupIndices(name));
 }

◆ useIndices()

void TSR::useIndices ( const std::vector< std::size_t > & indices )

Use DoF indices for TSR computation.

Parameters

[in] indices Indices to use.

Definition at line 501 of file tsr.cpp.

 {
     indices_ = indices;
     if (ik_)
     {
         ik_->setDofs(indices_);
         indices_ = ik_->getDofs();
     }
  
     computeBijection();
 }

◆ useWorldIndices()

void TSR::useWorldIndices ( const std::vector< std::pair< std::size_t, std::size_t >> & indices )

Use World DoF indices for TSR computation. World indices are pairs of skeleton index and DoF index.

Parameters

[in] indices Indices to use.

Definition at line 513 of file tsr.cpp.

 {
     std::vector<std::size_t> use;
     for (const auto &index : indices)
     {
         if (index.first == getSkeletonIndex())
             use.emplace_back(index.second);
     }
  
     useIndices(use);
 }

Member Data Documentation

◆ bijection_

std::vector<std::size_t> robowflex::darts::TSR::bijection_

private

Mapping between controlled to world indices. The ith entry contains the mapping of state[i] = world[bijection_[i]]

Definition at line 698 of file tsr.h.

◆ frame_

std::shared_ptr<dart::dynamics::SimpleFrame> robowflex::darts::TSR::frame_ {nullptr}

private

Target frame of TSR.

Definition at line 702 of file tsr.h.

◆ ik_

std::shared_ptr<dart::dynamics::InverseKinematics> robowflex::darts::TSR::ik_ {nullptr}

private

Inverse kinematics module.

Definition at line 704 of file tsr.h.

◆ indices_

std::vector<std::size_t> robowflex::darts::TSR::indices_

private

Controlled indices.

Definition at line 696 of file tsr.h.

◆ skel_index_

std::size_t robowflex::darts::TSR::skel_index_

private

Index of controlled skeleton.

Definition at line 695 of file tsr.h.

◆ spec_

Specification robowflex::darts::TSR::spec_

private

TSR specification.

Definition at line 693 of file tsr.h.

◆ tnd_

dart::dynamics::BodyNode* robowflex::darts::TSR::tnd_ {nullptr}

private

Target body node.

Definition at line 703 of file tsr.h.

◆ tsr_

dart::dynamics::InverseKinematics::TaskSpaceRegion* robowflex::darts::TSR::tsr_ {nullptr}

private

TSR.

Definition at line 705 of file tsr.h.

◆ world_

WorldPtr robowflex::darts::TSR::world_

private

Underlying world.

Definition at line 692 of file tsr.h.

◆ world_indices_

std::vector<std::pair<std::size_t, std::size_t> > robowflex::darts::TSR::world_indices_

private

World indices.

Definition at line 697 of file tsr.h.

The documentation for this class was generated from the following files:

Classes

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ TSR()

◆ ~TSR()

Member Function Documentation

◆ clear()

◆ computeBijection()

◆ computeWorldIndices()

◆ distance()

◆ distanceWorld()

◆ distanceWorldState()

◆ fromBijection()

◆ getDimension()

◆ getError()

◆ getErrorWorld()

◆ getErrorWorldRaw()

◆ getErrorWorldState()

◆ getIndices()

◆ getJacobian()

◆ getJacobianWorld()

◆ getJacobianWorldState()

◆ getNumDofs()

◆ getNumWorldDofs()

◆ getPositions()

◆ getPositionsWorldState()

◆ getSkeletonIndex()

◆ getSpecification()

◆ getTransformToFrame()

◆ getWorldIndices()

◆ initialize()

◆ setPositions()

◆ setPositionsWorldState()

◆ setWorld()

◆ setWorldIndices()

◆ solve()

◆ solveGradient()

◆ solveGradientWorld()

◆ solveGradientWorldState()

◆ solveWorld()

◆ solveWorldState()

◆ toBijection()

◆ updateBounds()

◆ updatePose()

◆ updateSolver()

◆ useGroup()

◆ useIndices()

◆ useWorldIndices()

Member Data Documentation

◆ bijection_

◆ frame_

◆ ik_

◆ indices_

◆ skel_index_

◆ spec_

◆ tnd_

◆ tsr_

◆ world_

◆ world_indices_