Tesseract 0.28.4
Loading...
Searching...
No Matches
utils.cpp File Reference

Tesseract Environment Utility Functions. More...

Typedefs

using tesseract::environment::CalcStateFn = std::function<tesseract::common::LinkIdTransformMap(const Eigen::VectorXd& state)>

Functions

void tesseract::environment::getActiveLinkIdsRecursive (std::vector< tesseract::common::LinkId > &active_links, const tesseract::scene_graph::SceneGraph &scene_graph, const tesseract::common::LinkId &current_link, bool active)
 Get the active link ids Recursively.
void tesseract::environment::checkTrajectorySegment (tesseract::collision::ContactResultMap &contact_results, tesseract::collision::ContinuousContactManager &manager, const tesseract::common::LinkIdTransformMap &state0, const tesseract::common::LinkIdTransformMap &state1, const tesseract::collision::ContactRequest &contact_request)
 Should perform a continuous collision check between two states only passing along the contact_request to the manager.
void tesseract::environment::checkTrajectoryState (tesseract::collision::ContactResultMap &contact_results, tesseract::collision::DiscreteContactManager &manager, const tesseract::common::LinkIdTransformMap &state, const tesseract::collision::ContactRequest &contact_request)
 Should perform a discrete collision check a state only passing contact_request to the manager.
void tesseract::environment::checkTrajectoryState (tesseract::collision::ContactResultMap &contact_results, tesseract::collision::ContinuousContactManager &manager, const tesseract::common::LinkIdTransformMap &state, const tesseract::collision::ContactRequest &contact_request)
 Should perform a discrete collision check a state first configuring manager with config.
void tesseract::environment::printContinuousDebugInfo (const std::vector< std::string > &joint_names, const Eigen::VectorXd &swp0, const Eigen::VectorXd &swp1, tesseract::common::TrajArray::Index step_idx, tesseract::common::TrajArray::Index step_size, tesseract::common::TrajArray::Index sub_step_idx=-1)
void tesseract::environment::printDiscreteDebugInfo (const std::vector< std::string > &joint_names, const Eigen::VectorXd &swp, tesseract::common::TrajArray::Index step_idx, tesseract::common::TrajArray::Index step_size, tesseract::common::TrajArray::Index sub_step_idx=-1)
tesseract::collision::ContactTrajectoryResults tesseract::environment::checkTrajectory (std::vector< tesseract::collision::ContactResultMap > &contacts, tesseract::collision::ContinuousContactManager &manager, const CalcStateFn &state_fn, const std::vector< tesseract::common::JointId > &joint_ids, const tesseract::common::TrajArray &traj, const tesseract::collision::CollisionCheckConfig &config)
tesseract::collision::ContactTrajectoryResults tesseract::environment::checkTrajectory (std::vector< tesseract::collision::ContactResultMap > &contacts, tesseract::collision::ContinuousContactManager &manager, const tesseract::scene_graph::StateSolver &state_solver, const std::vector< tesseract::common::JointId > &joint_ids, const tesseract::common::TrajArray &traj, const tesseract::collision::CollisionCheckConfig &config)
 Should perform a continuous collision check over the trajectory and stop on first collision.
tesseract::collision::ContactTrajectoryResults tesseract::environment::checkTrajectory (std::vector< tesseract::collision::ContactResultMap > &contacts, tesseract::collision::ContinuousContactManager &manager, const tesseract::kinematics::JointGroup &manip, const tesseract::common::TrajArray &traj, const tesseract::collision::CollisionCheckConfig &config)
 Should perform a continuous collision check over the trajectory and stop on first collision.
tesseract::collision::ContactTrajectoryResults tesseract::environment::checkTrajectory (std::vector< tesseract::collision::ContactResultMap > &contacts, tesseract::collision::DiscreteContactManager &manager, const CalcStateFn &state_fn, const std::vector< tesseract::common::JointId > &joint_ids, const tesseract::common::TrajArray &traj, const tesseract::collision::CollisionCheckConfig &config)
tesseract::collision::ContactTrajectoryResults tesseract::environment::checkTrajectory (std::vector< tesseract::collision::ContactResultMap > &contacts, tesseract::collision::DiscreteContactManager &manager, const tesseract::scene_graph::StateSolver &state_solver, const std::vector< tesseract::common::JointId > &joint_ids, const tesseract::common::TrajArray &traj, const tesseract::collision::CollisionCheckConfig &config)
 Should perform a discrete collision check over the trajectory and stop on first collision.
tesseract::collision::ContactTrajectoryResults tesseract::environment::checkTrajectory (std::vector< tesseract::collision::ContactResultMap > &contacts, tesseract::collision::DiscreteContactManager &manager, const tesseract::kinematics::JointGroup &manip, const tesseract::common::TrajArray &traj, const tesseract::collision::CollisionCheckConfig &config)
 Should perform a discrete collision check over the trajectory and stop on first collision.

Detailed Description

Tesseract Environment Utility Functions.

Author
Levi Armstrong
Date
Dec 18, 2017
License
Software License Agreement (Apache License)
Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

Function Documentation

◆ getActiveLinkIdsRecursive()

void tesseract::environment::getActiveLinkIdsRecursive ( std::vector< tesseract::common::LinkId > & active_links,
const tesseract::scene_graph::SceneGraph & scene_graph,
const tesseract::common::LinkId & current_link,
bool active )

Get the active link ids Recursively.

Get the active Link Names Recursively.

   This currently only works for graphs that are trees. Need to create a generic method using boost::visitor
   TODO: Need to update using graph->getLinkChildren
Parameters
active_links
scene_graph
current_link
active

◆ checkTrajectorySegment()

void tesseract::environment::checkTrajectorySegment ( tesseract::collision::ContactResultMap & contact_results,
tesseract::collision::ContinuousContactManager & manager,
const tesseract::common::LinkIdTransformMap & state0,
const tesseract::common::LinkIdTransformMap & state1,
const tesseract::collision::ContactRequest & contact_request )

Should perform a continuous collision check between two states only passing along the contact_request to the manager.

Parameters
contact_resultsThe contact results to populate. It does not get cleared
managerA continuous contact manager
state0First environment state
state1Second environment state
contact_requestContact request passed to the manager

◆ checkTrajectoryState() [1/2]

void tesseract::environment::checkTrajectoryState ( tesseract::collision::ContactResultMap & contact_results,
tesseract::collision::DiscreteContactManager & manager,
const tesseract::common::LinkIdTransformMap & state,
const tesseract::collision::ContactRequest & contact_request )

Should perform a discrete collision check a state only passing contact_request to the manager.

Parameters
contact_resultsThe contact results to populate. It does not get cleared
managerA discrete contact manager
stateEnvironment state
contact_requestContact request passed to the manager

◆ checkTrajectoryState() [2/2]

void tesseract::environment::checkTrajectoryState ( tesseract::collision::ContactResultMap & contact_results,
tesseract::collision::ContinuousContactManager & manager,
const tesseract::common::LinkIdTransformMap & state,
const tesseract::collision::ContactRequest & contact_request )

Should perform a discrete collision check a state first configuring manager with config.

Parameters
contact_resultsThe contact results to populate. It does not get cleared
managerA continuous contact manager
stateEnvironment state
contact_requestContact request passed to the manager

◆ checkTrajectory() [1/6]

tesseract::collision::ContactTrajectoryResults tesseract::environment::checkTrajectory ( std::vector< tesseract::collision::ContactResultMap > & contacts,
tesseract::collision::ContinuousContactManager & manager,
const CalcStateFn & state_fn,
const std::vector< tesseract::common::JointId > & joint_ids,
const tesseract::common::TrajArray & traj,
const tesseract::collision::CollisionCheckConfig & config )

Making this thread_local does not help because it is not called enough during planning

◆ checkTrajectory() [2/6]

tesseract::collision::ContactTrajectoryResults tesseract::environment::checkTrajectory ( std::vector< tesseract::collision::ContactResultMap > & contacts,
tesseract::collision::ContinuousContactManager & manager,
const tesseract::scene_graph::StateSolver & state_solver,
const std::vector< tesseract::common::JointId > & joint_ids,
const tesseract::common::TrajArray & traj,
const tesseract::collision::CollisionCheckConfig & config )

Should perform a continuous collision check over the trajectory and stop on first collision.

Parameters
contactsA vector of ContactMap where each index corresponds to a segment in the trajectory.
managerA continuous contact manager
state_solverThe environment state solver
joint_idsJointIds corresponding to the values in traj (must be in same order)
trajThe joint values at each time step
configCollisionCheckConfig used to specify collision check settings
Returns
ContactTrajectoryResults containing contact step/substep locations and joint values.

◆ checkTrajectory() [3/6]

tesseract::collision::ContactTrajectoryResults tesseract::environment::checkTrajectory ( std::vector< tesseract::collision::ContactResultMap > & contacts,
tesseract::collision::ContinuousContactManager & manager,
const tesseract::kinematics::JointGroup & manip,
const tesseract::common::TrajArray & traj,
const tesseract::collision::CollisionCheckConfig & config )

Should perform a continuous collision check over the trajectory and stop on first collision.

Parameters
contactsA vector of ContactMap where each index corresponds to a segment in the trajectory. The length should be trajectory size minus one.
managerA continuous contact manager
manipThe kinematic joint group
trajThe joint values at each time step
configCollisionCheckConfig used to specify collision check settings
Returns
ContactTrajectoryResults containing contact step/substep locations and joint values.

◆ checkTrajectory() [4/6]

tesseract::collision::ContactTrajectoryResults tesseract::environment::checkTrajectory ( std::vector< tesseract::collision::ContactResultMap > & contacts,
tesseract::collision::DiscreteContactManager & manager,
const CalcStateFn & state_fn,
const std::vector< tesseract::common::JointId > & joint_ids,
const tesseract::common::TrajArray & traj,
const tesseract::collision::CollisionCheckConfig & config )

Making this thread_local does not help because it is not called enough during planning

◆ checkTrajectory() [5/6]

tesseract::collision::ContactTrajectoryResults tesseract::environment::checkTrajectory ( std::vector< tesseract::collision::ContactResultMap > & contacts,
tesseract::collision::DiscreteContactManager & manager,
const tesseract::scene_graph::StateSolver & state_solver,
const std::vector< tesseract::common::JointId > & joint_ids,
const tesseract::common::TrajArray & traj,
const tesseract::collision::CollisionCheckConfig & config )

Should perform a discrete collision check over the trajectory and stop on first collision.

Parameters
contactsA vector of ContactMap where each index corresponds to a segment in the trajectory.
managerA discrete contact manager
state_solverThe environment state solver
joint_idsJointIds corresponding to the values in traj (must be in same order)
trajThe joint values at each time step
configCollisionCheckConfig used to specify collision check settings
Returns
ContactTrajectoryResults containing contact step/substep locations and joint values.

◆ checkTrajectory() [6/6]

tesseract::collision::ContactTrajectoryResults tesseract::environment::checkTrajectory ( std::vector< tesseract::collision::ContactResultMap > & contacts,
tesseract::collision::DiscreteContactManager & manager,
const tesseract::kinematics::JointGroup & manip,
const tesseract::common::TrajArray & traj,
const tesseract::collision::CollisionCheckConfig & config )

Should perform a discrete collision check over the trajectory and stop on first collision.

Parameters
contactsA vector of ContactMap where each index corresponds to a segment in the trajectory, except the last which is the end state. The length should be the same size as the input trajectory.
managerA continuous contact manager
manipThe kinematic joint group
trajThe joint values at each time step
configCollisionCheckConfig used to specify collision check settings
Returns
ContactTrajectoryResults containing contact step/substep locations and joint values.