Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Waypoint Definition Update #125

Merged
merged 5 commits into from
Oct 2, 2019
Merged

Waypoint Definition Update #125

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
864e8de
Updated joint toleranced waypoint to inherit from joint waypoint
marip8 Sep 17, 2019
a52ad6c
Updated Cartesian waypoint to hold a link relative to which its trans…
marip8 Sep 17, 2019
22bd743
Updates to generators and examples to utilize cartesian pose getParen…
marip8 Sep 17, 2019
e81b270
Clang formatting
marip8 Sep 19, 2019
184f767
Updated TrajOpt planner unit test
marip8 Oct 1, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion ...sseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ inline std::vector<Waypoint::Ptr> interpolate(const Waypoint& start, const Waypo
for (auto& eigen_pose : eigen_poses)
{
CartesianWaypoint::Ptr new_waypoint =
std::make_shared<tesseract_motion_planners::CartesianWaypoint>(eigen_pose);
std::make_shared<tesseract_motion_planners::CartesianWaypoint>(eigen_pose, w1.getParentLinkName());
new_waypoint->setCoefficients(start.getCoefficients());
new_waypoint->setIsCritical(start.isCritical());
result.push_back(new_waypoint);
Expand Down
47 changes: 22 additions & 25 deletions ...ract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/waypoint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,7 @@ class Waypoint
*/
bool is_critical_ = true;
};

/** @brief Defines a joint position waypoint for use with Tesseract Planners*/
class JointWaypoint : public Waypoint
{
Expand Down Expand Up @@ -156,9 +157,10 @@ class JointWaypoint : public Waypoint
const std::vector<std::string>& getNames() const { return joint_names_; }

protected:
Eigen::VectorXd joint_positions_; /**< @brief Joint position in radians */
std::vector<std::string> joint_names_; /**< @brief Joint names */
Eigen::VectorXd joint_positions_; /** @brief Joint position in radians */
std::vector<std::string> joint_names_; /** @brief Joint names */
};

/** @brief Defines a cartesian position waypoint for use with Tesseract Planners */
class CartesianWaypoint : public Waypoint
{
Expand All @@ -168,14 +170,16 @@ class CartesianWaypoint : public Waypoint
using Ptr = std::shared_ptr<CartesianWaypoint>;
using ConstPtr = std::shared_ptr<const CartesianWaypoint>;

CartesianWaypoint(Eigen::Isometry3d cartesian_position)
: Waypoint(WaypointType::CARTESIAN_WAYPOINT), cartesian_position_(std::move(cartesian_position))
CartesianWaypoint(Eigen::Isometry3d cartesian_position, std::string parent_link = "")
: Waypoint(WaypointType::CARTESIAN_WAYPOINT)
, cartesian_position_(std::move(cartesian_position))
, parent_link_(std::move(parent_link))
{
setCoefficients(Eigen::VectorXd::Ones(6));
}

CartesianWaypoint(Eigen::Vector3d position, Eigen::Quaterniond orientation)
: Waypoint(WaypointType::CARTESIAN_WAYPOINT)
CartesianWaypoint(Eigen::Vector3d position, Eigen::Quaterniond orientation, std::string link = "")
: Waypoint(WaypointType::CARTESIAN_WAYPOINT), parent_link_(std::move(link))
{
cartesian_position_.translation() = position;
cartesian_position_.linear() = orientation.toRotationMatrix();
Expand All @@ -189,48 +193,44 @@ class CartesianWaypoint : public Waypoint
const Eigen::Isometry3d& getTransform() const { return cartesian_position_; }

/** @brief Convenience function that returns the xyz cartesian position contained in cartesian_position_ */
Eigen::Vector3d getPosition() { return cartesian_position_.translation(); }
Eigen::Vector3d getPosition() const { return cartesian_position_.translation(); }
/**
* @brief Convenience function that returns the wxyz rotation quarternion contained in cartesian_position
* @return Quaternion(w, x, y, z)
*/
Eigen::Vector4d getOrientation()
Eigen::Vector4d getOrientation() const
{
Eigen::Quaterniond q(cartesian_position_.rotation());
return Eigen::Vector4d(q.w(), q.x(), q.y(), q.z());
}

/** @brief Gets the name of the link to which this position is relative */
std::string getParentLinkName() const { return parent_link_; }

protected:
Eigen::Isometry3d cartesian_position_; /**< @brief Pose of this waypoint */
Eigen::Isometry3d cartesian_position_; /** @brief Pose of this waypoint */
std::string parent_link_; /** @brief The link to which this position is defined */
};

/** @brief Defines a joint toleranced position waypoint for use with Tesseract Planners*/
class JointTolerancedWaypoint : public Waypoint
class JointTolerancedWaypoint : public JointWaypoint
{
public:
using Ptr = std::shared_ptr<JointTolerancedWaypoint>;
using ConstPtr = std::shared_ptr<const JointTolerancedWaypoint>;

JointTolerancedWaypoint(Eigen::VectorXd joint_positions, std::vector<std::string> joint_names)
: Waypoint(WaypointType::JOINT_TOLERANCED_WAYPOINT)
, joint_positions_(std::move(joint_positions))
, joint_names_(std::move(joint_names))
: JointWaypoint(joint_positions, joint_names)
{
assert(joint_positions_.size() == static_cast<long>(joint_names_.size()));
setCoefficients(Eigen::VectorXd::Ones(joint_positions_.size()));
waypoint_type_ = WaypointType::JOINT_TOLERANCED_WAYPOINT;
setUpperTolerance(Eigen::VectorXd::Zero(joint_positions_.size()));
setLowerTolerance(Eigen::VectorXd::Zero(joint_positions_.size()));
}

JointTolerancedWaypoint(std::vector<double> joint_positions, std::vector<std::string> joint_names)
: Waypoint(WaypointType::JOINT_TOLERANCED_WAYPOINT), joint_names_(std::move(joint_names))
: JointWaypoint(joint_positions, joint_names)
{
joint_positions_.resize(static_cast<long>(joint_positions.size()));
for (long i = 0; i < static_cast<long>(joint_positions.size()); ++i)
joint_positions_[i] = joint_positions[static_cast<size_t>(i)];

assert(joint_positions_.size() == static_cast<long>(joint_names_.size()));
setCoefficients(Eigen::VectorXd::Ones(joint_positions_.size()));
waypoint_type_ = WaypointType::JOINT_TOLERANCED_WAYPOINT;
setUpperTolerance(Eigen::VectorXd::Zero(joint_positions_.size()));
setLowerTolerance(Eigen::VectorXd::Zero(joint_positions_.size()));
}
Expand Down Expand Up @@ -294,9 +294,6 @@ class JointTolerancedWaypoint : public Waypoint
const Eigen::VectorXd& getLowerTolerance() const { return lower_tolerance_; }

protected:
Eigen::VectorXd joint_positions_; /**< @brief Joint position in radians */
std::vector<std::string> joint_names_; /**< @brief Joint names */

/** @brief Amount over joint_positions_ that is allowed (positive radians). */
Eigen::VectorXd upper_tolerance_;

Expand Down
24 changes: 12 additions & 12 deletions tesseract/tesseract_planning/tesseract_motion_planners/test/trajopt_planner_tests.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -239,8 +239,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespacePlanner2)
std::make_shared<JointWaypoint>(std::vector<double>({ 0, 0, 0, -1.57, 0, 0, 0 }), joint_names);

// Specify a Cartesian Waypoint as the finish
config.end_waypoint_ =
std::make_shared<CartesianWaypoint>(Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0));
config.end_waypoint_ = std::make_shared<CartesianWaypoint>(
Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0), "base_link");

// Create test planner used for testing problem creation
tesseract_tests::TrajOptFreespacePlannerTest test_planner;
Expand Down Expand Up @@ -284,8 +284,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespacePlanner3)
tesseract_ptr_->getFwdKinematicsManagerConst()->getFwdKinematicSolver("manipulator")->getJointNames();

// Specify a Cartesian Waypoint as the start
config.start_waypoint_ =
std::make_shared<CartesianWaypoint>(Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0));
config.start_waypoint_ = std::make_shared<CartesianWaypoint>(
Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0), "base_link");

// Specify a Joint Waypoint as the finish
config.end_waypoint_ = std::make_shared<JointWaypoint>(std::vector<double>({ 0, 0, 0, -1.57, 0, 0, 0 }), joint_names);
Expand Down Expand Up @@ -329,12 +329,12 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespacePlanner4)
config.num_steps_ = NUM_STEPS;

// Specify a Cartesian Waypoint as the start
config.start_waypoint_ =
std::make_shared<CartesianWaypoint>(Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0));
config.start_waypoint_ = std::make_shared<CartesianWaypoint>(
Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0), "base_link");

// Specify a Cartesian Waypoint as the finish
config.end_waypoint_ =
std::make_shared<CartesianWaypoint>(Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0));
config.end_waypoint_ = std::make_shared<CartesianWaypoint>(
Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0), "base_link");

// Create test planner used for testing problem creation
tesseract_tests::TrajOptFreespacePlannerTest test_planner;
Expand Down Expand Up @@ -377,8 +377,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayPlanner0)
// These specify the series of points to be optimized
for (int ind = 0; ind < NUM_STEPS; ind++)
{
CartesianWaypoint::Ptr waypoint = std::make_shared<CartesianWaypoint>(Eigen::Vector3d(-0.2 + ind * .02, 0.4, 0.8),
Eigen::Quaterniond(0, 1.0, 0, 0));
CartesianWaypoint::Ptr waypoint = std::make_shared<CartesianWaypoint>(
Eigen::Vector3d(-0.2 + ind * .02, 0.4, 0.8), Eigen::Quaterniond(0, 1.0, 0, 0), "base_link");
waypoint->setIsCritical(true);
config.target_waypoints_.push_back(waypoint);
}
Expand Down Expand Up @@ -428,8 +428,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayPlanner1)
// These specify the series of points to be optimized
for (int ind = 0; ind < NUM_STEPS; ind++)
{
CartesianWaypoint::Ptr waypoint = std::make_shared<CartesianWaypoint>(Eigen::Vector3d(-0.2 + ind * .02, 0.4, 0.8),
Eigen::Quaterniond(0, 1.0, 0, 0));
CartesianWaypoint::Ptr waypoint = std::make_shared<CartesianWaypoint>(
Eigen::Vector3d(-0.2 + ind * .02, 0.4, 0.8), Eigen::Quaterniond(0, 1.0, 0, 0), "base_link");
waypoint->setIsCritical(false);
config.target_waypoints_.push_back(waypoint);
}
Expand Down
3 changes: 2 additions & 1 deletion ...process_planners/include/tesseract_process_planners/generators/axial_approach_generator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,8 @@ class AxialApproachGenerator : public ProcessStepGenerator

tesseract_motion_planners::CartesianWaypoint::Ptr new_waypoint =
std::make_shared<tesseract_motion_planners::CartesianWaypoint>(
config.world_offset_direction * cur_waypoint->getTransform() * config.local_offset_direction * scaled);
config.world_offset_direction * cur_waypoint->getTransform() * config.local_offset_direction * scaled,
cur_waypoint->getParentLinkName());
new_waypoint->setCoefficients(cur_waypoint->getCoefficients());
new_waypoint->setIsCritical(cur_waypoint->isCritical());
approach.push_back(new_waypoint);
Expand Down
3 changes: 2 additions & 1 deletion ...rocess_planners/include/tesseract_process_planners/generators/axial_departure_generator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,8 @@ class AxialDepartureGenerator : public ProcessStepGenerator

tesseract_motion_planners::CartesianWaypoint::Ptr new_waypoint =
std::make_shared<tesseract_motion_planners::CartesianWaypoint>(
config.world_offset_direction * cur_waypoint->getTransform() * config.local_offset_direction * scaled);
config.world_offset_direction * cur_waypoint->getTransform() * config.local_offset_direction * scaled,
cur_waypoint->getParentLinkName());
new_waypoint->setCoefficients(cur_waypoint->getCoefficients());
new_waypoint->setIsCritical(cur_waypoint->isCritical());
departure.push_back(new_waypoint);
Expand Down
3 changes: 2 additions & 1 deletion ...ss_planners/include/tesseract_process_planners/generators/passthrough_process_generator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,8 @@ class PassthroughProcessGenerator : public ProcessStepGenerator
std::static_pointer_cast<tesseract_motion_planners::CartesianWaypoint>(waypoint);
tesseract_motion_planners::CartesianWaypoint::Ptr new_waypoint =
std::make_shared<tesseract_motion_planners::CartesianWaypoint>(
config.world_offset_direction * cur_waypoint->getTransform() * config.local_offset_direction);
config.world_offset_direction * cur_waypoint->getTransform() * config.local_offset_direction,
cur_waypoint->getParentLinkName());
new_waypoint->setCoefficients(cur_waypoint->getCoefficients());
new_waypoint->setIsCritical(cur_waypoint->isCritical());
new_waypoints.push_back(new_waypoint);
Expand Down
15 changes: 10 additions & 5 deletions tesseract_ros/tesseract_examples/tesseract_ros_examples/config/basic_cartesian_plan.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,8 @@
"wxyz" : [0.0, 0.0, 1.0, 0.0],
"link" : "tool0",
"pos_coeffs" : [10, 10, 10],
"rot_coeffs" : [10, 10, 10]
"rot_coeffs" : [10, 10, 10],
"target" : "base_link"
}
},
{
Expand All @@ -50,7 +51,8 @@
"wxyz" : [0.0, 0.0, 1.0, 0.0],
"link" : "tool0",
"pos_coeffs" : [10, 10, 10],
"rot_coeffs" : [10, 10, 10]
"rot_coeffs" : [10, 10, 10],
"target" : "base_link"
}
},
{
Expand All @@ -63,7 +65,8 @@
"wxyz" : [0.0, 0.0, 1.0, 0.0],
"link" : "tool0",
"pos_coeffs" : [10, 10, 10],
"rot_coeffs" : [10, 10, 10]
"rot_coeffs" : [10, 10, 10],
"target" : "base_link"
}
},
{
Expand All @@ -76,7 +79,8 @@
"wxyz" : [0.0, 0.0, 1.0, 0.0],
"link" : "tool0",
"pos_coeffs" : [10, 10, 10],
"rot_coeffs" : [10, 10, 10]
"rot_coeffs" : [10, 10, 10],
"target" : "base_link"
}
},
{
Expand All @@ -89,7 +93,8 @@
"wxyz" : [0.0, 0.0, 1.0, 0.0],
"link" : "tool0",
"pos_coeffs" : [10, 10, 10],
"rot_coeffs" : [10, 10, 10]
"rot_coeffs" : [10, 10, 10],
"target" : "base_link"
}
}
],
Expand Down
15 changes: 10 additions & 5 deletions tesseract_ros/tesseract_examples/tesseract_ros_examples/config/glass_up_right_plan.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,8 @@
"wxyz" : [0.0, 0.0, 1.0, 0.0],
"link" : "tool0",
"pos_coeffs" : [10, 10, 10],
"rot_coeffs" : [10, 10, 10]
"rot_coeffs" : [10, 10, 10],
"target" : "base_link"
}
},
{
Expand All @@ -65,7 +66,8 @@
"wxyz" : [0.0, 0.0, 1.0, 0.0],
"link" : "tool0",
"pos_coeffs" : [0, 0, 0],
"rot_coeffs" : [10, 10, 0]
"rot_coeffs" : [10, 10, 0],
"target" : "base_link"
}
},
{
Expand All @@ -78,7 +80,8 @@
"wxyz" : [0.0, 0.0, 1.0, 0.0],
"link" : "tool0",
"pos_coeffs" : [0, 0, 0],
"rot_coeffs" : [10, 10, 0]
"rot_coeffs" : [10, 10, 0],
"target" : "base_link"
}
},
{
Expand All @@ -91,7 +94,8 @@
"wxyz" : [0.0, 0.0, 1.0, 0.0],
"link" : "tool0",
"pos_coeffs" : [0, 0, 0],
"rot_coeffs" : [10, 10, 0]
"rot_coeffs" : [10, 10, 0],
"target" : "base_link"
}
},
{
Expand All @@ -104,7 +108,8 @@
"wxyz" : [0.0, 0.0, 1.0, 0.0],
"link" : "tool0",
"pos_coeffs" : [10, 10, 10],
"rot_coeffs" : [10, 10, 0]
"rot_coeffs" : [10, 10, 0],
"target" : "base_link"
}
}
],
Expand Down