Skip to content

Commit

Permalink
Add tolerances to joint/cartesian waypoints
Browse files Browse the repository at this point in the history
  • Loading branch information
@mpowelson
mpowelson committed Dec 31, 2020
1 parent 08f7ae3 commit 7b61ed7
Show file tree
Hide file tree
Showing 8 changed files with 96 additions and 12 deletions.
19 changes: 19 additions & 0 deletions ...anning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -264,6 +264,25 @@ class CartesianWaypoint

/** @brief The Cartesian Waypoint */
Eigen::Isometry3d waypoint;
/** @brief Distance below waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()) */
Eigen::VectorXd lower_tolerance;
/** @brief Distance above waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle())*/
Eigen::VectorXd upper_tolerance;

bool isToleranced() const
{
// Check if they are empty
if (lower_tolerance.size() == 0 || upper_tolerance.size() == 0)
return false;

// Check if they are close to 0
Eigen::VectorXd range = upper_tolerance - lower_tolerance;
double sum = range.sum();
if (sum < 1e-5)
return false;

return true;
}
};

} // namespace tesseract_planning
Expand Down
21 changes: 20 additions & 1 deletion ...t_planning/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -265,13 +265,32 @@ class JointWaypoint
inline operator Eigen::Ref<Eigen::VectorXd>() { return Eigen::Ref<Eigen::VectorXd>(waypoint); }

//////////////////////////////////
// Cartesian Waypoint Container //
// Joint Waypoint Container //
//////////////////////////////////

#endif // SWIG

Eigen::VectorXd waypoint;
std::vector<std::string> joint_names;
/** @brief Joint distance below waypoint that is allowed. Each element should be <= 0 */
Eigen::VectorXd lower_tolerance;
/** @brief Joint distance above waypoint that is allowed. Each element should be >= 0 */
Eigen::VectorXd upper_tolerance;

bool isToleranced() const
{
// Check if they are empty
if (lower_tolerance.size() == 0 || upper_tolerance.size() == 0)
return false;

// Check if they are close to 0
Eigen::VectorXd range = upper_tolerance - lower_tolerance;
double sum = range.sum();
if (sum < 1e-5)
return false;

return true;
}
};
} // namespace tesseract_planning

Expand Down
6 changes: 4 additions & 2 deletions ...planners/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <Eigen/Geometry>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_command_language/cartesian_waypoint.h>
#include <tesseract_command_language/joint_waypoint.h>
#include <tesseract_motion_planners/trajopt/profile/trajopt_profile.h>

#ifdef SWIG
Expand Down Expand Up @@ -81,14 +83,14 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile
constraint_error_functions;

void apply(trajopt::ProblemConstructionInfo& pci,
const Eigen::Isometry3d& cartesian_waypoint,
const CartesianWaypoint& cartesian_waypoint,
const Instruction& parent_instruction,
const ManipulatorInfo& manip_info,
const std::vector<std::string>& active_links,
int index) const override;

void apply(trajopt::ProblemConstructionInfo& pci,
const Eigen::VectorXd& joint_waypoint,
const JointWaypoint& joint_waypoint,
const Instruction& parent_instruction,
const ManipulatorInfo& manip_info,
const std::vector<std::string>& active_links,
Expand Down
4 changes: 2 additions & 2 deletions ...eract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,14 +58,14 @@ class TrajOptPlanProfile
TrajOptPlanProfile& operator=(TrajOptPlanProfile&&) = default;

virtual void apply(trajopt::ProblemConstructionInfo& pci,
const Eigen::Isometry3d& cartesian_waypoint,
const CartesianWaypoint& cartesian_waypoint,
const Instruction& parent_instruction,
const ManipulatorInfo& manip_info,
const std::vector<std::string>& active_links,
int index) const = 0;

virtual void apply(trajopt::ProblemConstructionInfo& pci,
const Eigen::VectorXd& joint_waypoint,
const JointWaypoint& joint_waypoint,
const Instruction& parent_instruction,
const ManipulatorInfo& manip_info,
const std::vector<std::string>& active_links,
Expand Down
8 changes: 8 additions & 0 deletions ...nning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,14 @@ trajopt::TermInfo::Ptr createJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
const Eigen::VectorXd& coeffs,
trajopt::TermType type);

trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
const Eigen::VectorXd& lower_tol,
const Eigen::VectorXd& upper_tol,
int index,
const Eigen::VectorXd& coeffs,
trajopt::TermType type);


trajopt::TermInfo::Ptr createCollisionTermInfo(
int start_index,
int end_index,
Expand Down
11 changes: 7 additions & 4 deletions ...ng/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,8 +138,9 @@ DefaultTrajoptProblemGenerator(const std::string& name,
}
else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
{
const Eigen::VectorXd& position = getJointPosition(start_waypoint);
start_plan_profile->apply(*pci, position, *start_instruction, composite_mi, active_links, index);
JointWaypoint position_wp;
position_wp.waypoint = getJointPosition(start_waypoint);
start_plan_profile->apply(*pci, position_wp, *start_instruction, composite_mi, active_links, index);
}
else
{
Expand Down Expand Up @@ -225,7 +226,8 @@ DefaultTrajoptProblemGenerator(const std::string& name,
}
else if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
{
const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
JointWaypoint cur_position;
cur_position.waypoint = getJointPosition(plan_instruction->getWaypoint());
Eigen::Isometry3d cur_pose = Eigen::Isometry3d::Identity();
if (!pci->kin->calcFwdKin(cur_pose, cur_position))
throw std::runtime_error("TrajOptPlannerUniversalConfig: failed to solve forward kinematics!");
Expand Down Expand Up @@ -283,7 +285,8 @@ DefaultTrajoptProblemGenerator(const std::string& name,
{
if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
{
const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
JointWaypoint cur_position;
cur_position.waypoint = getJointPosition(plan_instruction->getWaypoint());

// Add intermediate points with path costs and constraints
for (std::size_t s = 0; s < seed_composite->size() - 1; ++s)
Expand Down
14 changes: 11 additions & 3 deletions ...t_planning/tesseract_motion_planners/src/trajopt/profile/trajopt_default_plan_profile.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ TrajOptDefaultPlanProfile::TrajOptDefaultPlanProfile(const tinyxml2::XMLElement&
}
}
void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,
const Eigen::Isometry3d& cartesian_waypoint,
const CartesianWaypoint& cartesian_waypoint,
const Instruction& parent_instruction,
const ManipulatorInfo& manip_info,
const std::vector<std::string>& active_links,
Expand All @@ -119,6 +119,9 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,
auto it = std::find(active_links.begin(), active_links.end(), mi.working_frame);
if (it != active_links.end())
{
if (cartesian_waypoint.isToleranced())
CONSOLE_BRIDGE_logWarn("Toleranced cartesian waypoints are not supported in this version of TrajOpt.");

if (mi.tcp.isExternal())
{
// If external, the part is attached to the robot so working frame is passed as link instead of target frame
Expand Down Expand Up @@ -147,13 +150,18 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,
}

void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,
const Eigen::VectorXd& joint_waypoint,
const JointWaypoint& joint_waypoint,
const Instruction& /*parent_instruction*/,
const ManipulatorInfo& /*manip_info*/,
const std::vector<std::string>& /*active_links*/,
int index) const
{
auto ti = createJointWaypointTermInfo(joint_waypoint, index, joint_coeff, term_type);
trajopt::TermInfo::Ptr ti;
if (joint_waypoint.isToleranced())
ti = createTolerancedJointWaypointTermInfo(
joint_waypoint, joint_waypoint.lower_tolerance, joint_waypoint.upper_tolerance, index, joint_coeff, term_type);
else
ti = createJointWaypointTermInfo(joint_waypoint, index, joint_coeff, term_type);

if (term_type == trajopt::TermType::TT_CNT)
pci.cnt_infos.push_back(ti);
Expand Down
25 changes: 25 additions & 0 deletions tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,6 +138,31 @@ trajopt::TermInfo::Ptr createJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
return joint_info;
}

trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
const Eigen::VectorXd& lower_tol,
const Eigen::VectorXd& upper_tol,
int index,
const Eigen::VectorXd& coeffs,
trajopt::TermType type)
{
auto joint_info = std::make_shared<trajopt::JointPosTermInfo>();
if (coeffs.size() == 1)
joint_info->coeffs = std::vector<double>(static_cast<std::size_t>(j_wp.size()), coeffs(0));
else if (coeffs.size() == j_wp.size())
joint_info->coeffs = std::vector<double>(coeffs.data(), coeffs.data() + coeffs.rows() * coeffs.cols());

joint_info->targets = std::vector<double>(j_wp.data(), j_wp.data() + j_wp.rows() * j_wp.cols());
joint_info->lower_tols = std::vector<double>(lower_tol.data(), lower_tol.data() + lower_tol.rows() * lower_tol.cols());
joint_info->upper_tols = std::vector<double>(upper_tol.data(), upper_tol.data() + upper_tol.rows() * upper_tol.cols());
joint_info->first_step = index;
joint_info->last_step = index;
joint_info->name = "joint_waypoint_" + std::to_string(index);
joint_info->term_type = type;

return joint_info;
}


trajopt::TermInfo::Ptr createCollisionTermInfo(int start_index,
int end_index,
double collision_safety_margin,
Expand Down

0 comments on commit 7b61ed7

Please sign in to comment.