diff --git a/trajopt/CMakeLists.txt b/trajopt/CMakeLists.txt index c14b8fed..d488e478 100644 --- a/trajopt/CMakeLists.txt +++ b/trajopt/CMakeLists.txt @@ -83,4 +83,8 @@ if (CATKIN_ENABLE_TESTING) add_rostest_gtest(${PROJECT_NAME}_cast_cost_octomap_unit test/cast_cost_octomap_unit.launch test/cast_cost_octomap_unit.cpp) target_link_libraries(${PROJECT_NAME}_cast_cost_octomap_unit ${PROJECT_NAME} ${Boost_SYSTEM_LIBRARY} ${Boost_PROGRAM_OPTIONS_LIBRARY} ${PCL_LIBRARIES} ${catkin_LIBRARIES}) + + add_rostest_gtest(${PROJECT_NAME}_angular_constraint_unit test/angular_constraint_unit.launch test/angular_constraint_unit.cpp) + target_link_libraries(${PROJECT_NAME}_angular_constraint_unit ${PROJECT_NAME} ${Boost_SYSTEM_LIBRARY} ${Boost_PROGRAM_OPTIONS_LIBRARY} ${catkin_LIBRARIES}) + endif() diff --git a/trajopt/include/trajopt/kinematic_terms.hpp b/trajopt/include/trajopt/kinematic_terms.hpp index 4b0be7a5..eaf4bf73 100644 --- a/trajopt/include/trajopt/kinematic_terms.hpp +++ b/trajopt/include/trajopt/kinematic_terms.hpp @@ -1,5 +1,4 @@ #pragma once - #include #include #include @@ -104,4 +103,105 @@ struct CartVelCalculator : VectorOfVector VectorXd operator()(const VectorXd& dof_vals) const; }; + +/** + * @brief The AlignedAxisErrCalculator is a struct whose operator() calculates error of a given + * pose with respect to the rotation along the defined axis. + */ +struct AlignedAxisErrCalculator : public VectorOfVector +{ + Eigen::Matrix3d orientation_inv_; /**< @brief param The inverse of the desired orientation */ + tesseract::BasicKinConstPtr manip_; /**< @brief Kinematics object */ + tesseract::BasicEnvConstPtr env_; /**< @brief Environment object */ + std::string link_; /**< @brief Link of the robot referred to */ + Eigen::Matrix3d tcp_orientation_; /**< @brief Tool center point orientation */ + + Vector3d axis_; /**< @brief Axis of rotation to align with */ + double tol_; /**< @brief Tolerance angle in radians */ + + /** + * @brief AlignedAxisErrCalculator Constructor + * @param pose Desired pose + * @param manip + * @param env + * @param link + * @param axis Axis of rotation + * @param tol_angle Tolerance in radians + * @param tcp Tool center point + */ + AlignedAxisErrCalculator(const Eigen::Matrix3d& orientation, + tesseract::BasicKinConstPtr manip, + tesseract::BasicEnvConstPtr env, + std::string link, + Vector3d axis, + double tol, + Eigen::Matrix3d tcp_orientation = Eigen::Matrix3d::Identity()) + : orientation_inv_(orientation.inverse()) + , manip_(manip) + , env_(env) + , link_(link) + , tcp_orientation_(tcp_orientation) + , axis_(axis) + , tol_(tol) + { + } + + /** + * @brief operator () Calculates error + * @param dof_vals Values of the joints for forward kinematics + * @return 1D vector of error beyond the allowed rotation + */ + VectorXd operator()(const VectorXd& dof_vals) const; +}; + +/** + * @brief The ConicalAxisErrCalculator is a struct whose operator() returns the error of the + * given pose with respect to the conical constraint defined + * + */ +struct ConicalAxisErrCalculator : public VectorOfVector +{ + Eigen::Matrix3d orientation_inv_; /**< @brief Inverse of the desired orientation */ + tesseract::BasicKinConstPtr manip_; /**< @brief Kinematics object */ + tesseract::BasicEnvConstPtr env_; /**< @brief Environment object */ + std::string link_; /**< @brief The link of the robot referred to */ + Eigen::Matrix3d tcp_orientation_; /**< @brief Tool center point orientation */ + + Vector3d axis_; /**< @brief Axis the conical tolerance is applied to */ + double tol_; /**< @brief Tolerance angle in radians */ + + /** + * @brief ConicalAxisErrCalculator + * @param pose Desired pose + * @param manip Kinematics Object + * @param env Environment object + * @param link Link of the robot the term applies to + * @param axis Axis to define the conical tolerance around + * @param tol_angle Tolerance angle in degrees + * @param tcp Tool center point + */ + ConicalAxisErrCalculator(const Eigen::Matrix3d& orientation, + tesseract::BasicKinConstPtr manip, + tesseract::BasicEnvConstPtr env, + std::string link, + Vector3d axis, + double tol, + Eigen::Matrix3d tcp_orientation = Eigen::Matrix3d::Identity()) + : orientation_inv_(orientation.inverse()) + , manip_(manip) + , env_(env) + , link_(link) + , tcp_orientation_(tcp_orientation) + , axis_(axis) + , tol_(tol) + { + } + + /** + * @brief operator () + * @param dof_vals + * @return + */ + VectorXd operator()(const VectorXd& dof_vals) const; +}; } diff --git a/trajopt/include/trajopt/problem_description.hpp b/trajopt/include/trajopt/problem_description.hpp index a7182771..22e44ac4 100644 --- a/trajopt/include/trajopt/problem_description.hpp +++ b/trajopt/include/trajopt/problem_description.hpp @@ -4,6 +4,7 @@ #include #include #include +#include namespace sco { @@ -322,4 +323,67 @@ struct JointConstraintInfo : public TermInfo, public MakesConstraint void hatch(TrajOptProb& prob); DEFINE_CREATE(JointConstraintInfo) }; + +/** + * @brief The AlignedAxisTermInfo struct contains information to create constraints or costs + * using a tolerance for the rotation about an axis + */ +struct AlignedAxisTermInfo : public TermInfo, public MakesConstraint, public MakesCost +{ + int timestep; /**< @brief The timestep of this term in the trajectory */ + Vector4d + wxyz; /**< @brief 4D vector containing w, x, y, and z quaternion components for the pose orientation (in that + order) */ + Vector4d tcp_wxyz; /**< @brief Tool center point */ + double axis_coeff; /**< @brief Coefficient multiplied by the errors of the rotation axis from the specified axis */ + double angle_coeff; /**< @brief Coefficient multipleid by the angle of rotation past the tolerance */ + string link; /**< @brief Link of the robot the term refers to */ + Vector3d axis; /**< @brief Axis allowed to rotate with respect to the frame of #link */ + double tolerance; /**< @brief Rotation tolerance about the given axis in radians */ + + AlignedAxisTermInfo(); + + /** + * @brief fromJson Constructs the term from a Json file + */ + void fromJson(ProblemConstructionInfo& pci, const Value& v); + + /** + * @brief hatch Add the proper cost/constraint functions to the problem + * @param prob The optimization problems to add the term to + */ + void hatch(TrajOptProb& prob); + DEFINE_CREATE(AlignedAxisTermInfo) +}; + +/** + * @brief The ConicalAxisTermInfo struct contains information to create constraints or costs + * using a conical tolerance about an axis + */ +struct ConicalAxisTermInfo : public TermInfo, public MakesConstraint, public MakesCost +{ + int timestep; /**< @brief The timestep of this term in the trajectory */ + Vector4d + wxyz; /**< @brief 4D vector containing w, x, y, and z quaternion components for the pose orientation (in that + order) */ + Vector4d tcp_wxyz; /**< @brief Tool center point */ + double weight; /**< @brief Coefficient multiplied by the errors of the rotation axis from the specified axis */ + string link; /**< @brief Link of the robot the term refers to */ + Vector3d axis; /**< @brief Axis around which the conical tolerance will be applied */ + double tolerance; /**< @brief Tolerance angle of the cone in radians*/ + + ConicalAxisTermInfo(); + + /** + * @brief fromJson Constructs the term from a Json file + */ + void fromJson(ProblemConstructionInfo& pci, const Value& v); + + /** + * @brief hatch Add the proper cost/constraint functions to the problem + * @param prob The optimization problems to add the term to + */ + void hatch(TrajOptProb& prob); + DEFINE_CREATE(ConicalAxisTermInfo) +}; } diff --git a/trajopt/src/kinematic_terms.cpp b/trajopt/src/kinematic_terms.cpp index 2e17fe95..8c246163 100644 --- a/trajopt/src/kinematic_terms.cpp +++ b/trajopt/src/kinematic_terms.cpp @@ -88,8 +88,13 @@ VectorXd StaticCartPoseErrCalculator::operator()(const VectorXd& dof_vals) const env_->getState(manip_->getJointNames(), dof_vals)->transforms.at(manip_->getBaseLinkName()))); manip_->calcFwdKin(new_pose, change_base, dof_vals, link_, *state); + // calculate the err of the current pose Affine3d pose_err = pose_inv_ * (new_pose * tcp_); + + // get the quaternion representation of the rotation error Quaterniond q(pose_err.rotation()); + + // construct a 6D vector containing orientation and positional errors VectorXd err = concat(Vector3d(q.x(), q.y(), q.z()), pose_err.translation()); return err; } @@ -171,37 +176,46 @@ VectorXd CartVelCalculator::operator()(const VectorXd& dof_vals) const return out; } -#if 0 -CartVelConstraint::CartVelConstraint(const VarVector& step0vars, const VarVector& step1vars, RobotAndDOFPtr manip, KinBody::LinkPtr link, double distlimit) : - ConstraintFromFunc(VectorOfVectorPtr(new CartVelCalculator(manip, link, distlimit)), - MatrixOfVectorPtr(new CartVelJacCalculator(manip, link, distlimit)), concat(step0vars, step1vars), VectorXd::Ones(0), INEQ, "CartVel") -{} // TODO coeffs -#endif +VectorXd AlignedAxisErrCalculator::operator()(const VectorXd& dof_vals) const +{ + // calculate the current pose given the DOF values for the robot + Affine3d new_pose, change_base; + change_base = env_->getLinkTransform(manip_->getBaseLinkName()); + manip_->calcFwdKin(new_pose, change_base, dof_vals, link_, *env_->getState()); -#if 0 -struct UpErrorCalculator { - Vector3d dir_local_; - Vector3d goal_dir_world_; - RobotAndDOFPtr manip_; - OR::KinBody::LinkPtr link_; - MatrixXd perp_basis_; // 2x3 matrix perpendicular to goal_dir_world - UpErrorCalculator(const Vector3d& dir_local, const Vector3d& goal_dir_world, RobotAndDOFPtr manip, KinBody::LinkPtr link) : - dir_local_(dir_local), - goal_dir_world_(goal_dir_world), - manip_(manip), - link_(link) - { - Vector3d perp0 = goal_dir_world_.cross(Vector3d::Random()).normalized(); - Vector3d perp1 = goal_dir_world_.cross(perp0); - perp_basis_.resize(2,3); - perp_basis_.row(0) = perp0.transpose(); - perp_basis_.row(1) = perp1.transpose(); - } - VectorXd operator()(const VectorXd& dof_vals) { - manip_->SetDOFValues(toDblVec(dof_vals)); - OR::Transform newpose = link_->GetTransform(); - return perp_basis_*(toRot(newpose.rot) * dir_local_ - goal_dir_world_); - } -}; -#endif + // get the error of the current pose + Matrix3d orientation_err = orientation_inv_ * (new_pose.rotation() * tcp_orientation_); + AngleAxisd aa_err(orientation_err); + + // gets terms for error determination + double angle = fabs(aa_err.angle()); + + // the angle error is typical: actual value - tolerance + double angle_err = angle - tol_; + + // axis error + // the element wise squared difference between the desired axis and + // the desired axis after being rotated by the current pose "error" + Vector3d axis_err = (axis_ - (orientation_err * axis_)).array().square(); + + Vector4d err(axis_err.x(), axis_err.y(), axis_err.z(), angle_err); + return err; +} + +VectorXd ConicalAxisErrCalculator::operator()(const VectorXd& dof_vals) const +{ + // calculate the current pose given the DOF values for the robot + Affine3d new_pose, change_base; + change_base = env_->getLinkTransform(manip_->getBaseLinkName()); + manip_->calcFwdKin(new_pose, change_base, dof_vals, link_, *env_->getState()); + + // get the orientation matrix of the error + Matrix3d orientation_err = orientation_inv_ * (new_pose.rotation() * tcp_orientation_); + VectorXd err(1); + + // determine the error of the conical axis + err(0) = acos((orientation_err * axis_).dot(axis_)) - tol_; + + return err; +} } diff --git a/trajopt/src/problem_description.cpp b/trajopt/src/problem_description.cpp index 93b23aaa..db867227 100644 --- a/trajopt/src/problem_description.cpp +++ b/trajopt/src/problem_description.cpp @@ -55,6 +55,8 @@ void RegisterMakers() TermInfo::RegisterMaker("joint", &JointConstraintInfo::create); TermInfo::RegisterMaker("cart_vel", &CartVelCntInfo::create); TermInfo::RegisterMaker("joint_vel_limits", &JointVelConstraintInfo::create); + TermInfo::RegisterMaker("aligned_axis", &AlignedAxisTermInfo::create); + TermInfo::RegisterMaker("conical_axis", &ConicalAxisTermInfo::create); gRegisteredMakers = true; } @@ -801,4 +803,120 @@ void JointConstraintInfo::hatch(TrajOptProb& prob) prob.addLinearConstraint(exprSub(AffExpr(vars[j]), vals[j]), EQ); } } + +// initialize with basic default values +AlignedAxisTermInfo::AlignedAxisTermInfo() : tcp_wxyz(1.0, 0.0, 0.0, 0.0) +{ + axis_coeff = 0.0; + angle_coeff = 0.0; +} + +// get the term information from a json value +void AlignedAxisTermInfo::fromJson(ProblemConstructionInfo& pci, const Value& v) +{ + // make sure params is a member of the Json value provided + FAIL_IF_FALSE(v.isMember("params")); + + // get the params value and read the parameters fmro the file into the object members + const Value& params = v["params"]; + childFromJson(params, timestep, "timestep", pci.basic_info.n_steps - 1); + childFromJson(params, wxyz, "wxyz"); + childFromJson(params, tcp_wxyz, "tcp_xwyz", tcp_wxyz); + childFromJson(params, axis_coeff, "axis_coeff"); + childFromJson(params, angle_coeff, "angle_coeff"); + childFromJson(params, link, "link"); + childFromJson(params, axis, "axis"); + childFromJson(params, tolerance, "tolerance"); + + // make sure the link name provided is valid + const std::vector& link_names = pci.kin->getLinkNames(); + if (std::find(link_names.begin(), link_names.end(), link) == link_names.end()) + { + PRINT_AND_THROW(boost::format("invalid link name: %s") % link); + } + + // make sure there are no extra items in the params value + const char* all_fields[] = { "timestep", "wxyz", "axis_coeff", "angle_coeff", "link", "tolerance", "axis", "tcp_" + "wxyz" }; + ensure_only_members(params, all_fields, sizeof(all_fields) / sizeof(char*)); +} + +// add the term to the problem description +void AlignedAxisTermInfo::hatch(TrajOptProb& prob) +{ + // construct the desired pose + Eigen::Quaterniond input_q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); + Eigen::Quaterniond tcp_q(tcp_wxyz(0), tcp_wxyz(1), tcp_wxyz(2), tcp_wxyz(3)); + + // create the equality and inequality error functions to be used + VectorOfVectorPtr f_ineq(new AlignedAxisErrCalculator( + input_q.matrix(), prob.GetKin(), prob.GetEnv(), link, axis, tolerance, tcp_q.matrix())); + + Eigen::Vector4d coeffs(axis_coeff, axis_coeff, axis_coeff, angle_coeff); + + // add the costs or constraints depending on the term type + if (term_type == TT_COST) + { + prob.addCost(CostPtr(new CostFromErrFunc(f_ineq, prob.GetVarRow(timestep), coeffs, HINGE, name))); + } + else if (term_type == TT_CNT) + { + prob.addConstraint(ConstraintPtr(new ConstraintFromFunc(f_ineq, prob.GetVarRow(timestep), coeffs, INEQ, name))); + } +} + +// initialize with basic default values +ConicalAxisTermInfo::ConicalAxisTermInfo() : tcp_wxyz(1.0, 0.0, 0.0, 0.0) { weight = 0.0; } +// construct the term from a Json file +void ConicalAxisTermInfo::fromJson(ProblemConstructionInfo& pci, const Value& v) +{ + // make sure params is a member of the Json value provided + FAIL_IF_FALSE(v.isMember("params")); + + // get the params value and read the parameters fmro the file into the object members + const Value& params = v["params"]; + childFromJson(params, timestep, "timestep", pci.basic_info.n_steps - 1); + childFromJson(params, wxyz, "wxyz"); + childFromJson(params, tcp_wxyz, "tcp_xwyz", tcp_wxyz); + childFromJson(params, weight, "weight"); + childFromJson(params, link, "link"); + childFromJson(params, axis, "axis"); + childFromJson(params, tolerance, "tolerance"); + + // make sure the link name provided is valid + const std::vector& link_names = pci.kin->getLinkNames(); + if (std::find(link_names.begin(), link_names.end(), link) == link_names.end()) + { + PRINT_AND_THROW(boost::format("invalid link name: %s") % link); + } + + // make sure there are no extra items in the params value + const char* all_fields[] = { "timestep", "wxyz", "weight", "link", "tolerance", "axis", "tcp_wxyz" }; + ensure_only_members(params, all_fields, sizeof(all_fields) / sizeof(char*)); +} + +// add the term to the problem description +void ConicalAxisTermInfo::hatch(TrajOptProb& prob) +{ + // construct the desired pose + Eigen::Quaterniond input_q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); + Eigen::Quaterniond tcp_q(tcp_wxyz(0), tcp_wxyz(1), tcp_wxyz(2), tcp_wxyz(3)); + + // create the equality and inequality error functions to be used + VectorOfVectorPtr f_ineq(new ConicalAxisErrCalculator( + input_q.matrix(), prob.GetKin(), prob.GetEnv(), link, axis, tolerance, tcp_q.matrix())); + + VectorXd coeffs(1); + coeffs(0) = weight; + + // add the costs or constraints depending on the term type + if (term_type == TT_COST) + { + prob.addCost(CostPtr(new CostFromErrFunc(f_ineq, prob.GetVarRow(timestep), coeffs, HINGE, name))); + } + else if (term_type == TT_CNT) + { + prob.addConstraint(ConstraintPtr(new ConstraintFromFunc(f_ineq, prob.GetVarRow(timestep), coeffs, INEQ, name))); + } +} } diff --git a/trajopt/test/angular_constraint_unit.cpp b/trajopt/test/angular_constraint_unit.cpp new file mode 100644 index 00000000..ee9fb9f4 --- /dev/null +++ b/trajopt/test/angular_constraint_unit.cpp @@ -0,0 +1,371 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace trajopt; +using namespace tesseract; +using namespace Eigen; + +const std::string ROBOT_DESCRIPTION_PARAM = "robot_description"; /**< Default ROS parameter for robot description */ +const std::string ROBOT_SEMANTIC_PARAM = + "robot_description_semantic"; /**< Default ROS parameter for robot description */ + +bool plotting_ = false; +int steps_ = 5; +double approx_equals_diff_ = 1e-2; + +struct testInfo +{ + std::string method; + std::string constraint; + + testInfo(std::string m, std::string c) : method(m), constraint(c) {} +}; + +class AngularConstraintTest : public testing::TestWithParam +{ +public: + ros::NodeHandle nh_; + urdf::ModelInterfaceSharedPtr urdf_model_; /**< URDF Model */ + srdf::ModelSharedPtr srdf_model_; /**< SRDF Model */ + tesseract_ros::KDLEnvPtr env_; /**< Trajopt Basic Environment */ + tesseract_ros::ROSBasicPlottingPtr plotter_; /**< Trajopt Plotter */ + std::vector pose_inverses_; + std::string constraint_type_; + double tol_; + + AngularConstraintTest() : pose_inverses_(steps_) {} + TrajOptProbPtr jsonMethod() + { + std::string config_file; + nh_.getParam(constraint_type_ + "_json_file", config_file); + + Json::Value root; + Json::Reader reader; + bool parse_success = reader.parse(config_file.c_str(), root); + if (!parse_success) + { + ROS_FATAL("Failed to load trajopt json file from ros parameter"); + } + + ProblemConstructionInfo pci(env_); + pci.fromJson(root); + double y[] = { -0.2, -0.1, 0.0, 0.1, 0.2 }; + for (int i = 0; i < 5; i++) + { + Vector3d xyz(0.5, y[i], 0.62); + Vector4d wxyz(0.0, 0.0, 1.0, 0.0); + + Affine3d cur_pose; + cur_pose.translation() = xyz; + Quaterniond q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); + cur_pose.linear() = q.matrix(); + pose_inverses_.insert(pose_inverses_.begin() + i, cur_pose.inverse()); + } + + return ConstructProblem(pci); + } + + TrajOptProbPtr cppMethod() + { + ProblemConstructionInfo pci(env_); + + // Populate Basic Info + pci.basic_info.n_steps = steps_; + pci.basic_info.manip = "manipulator"; + pci.basic_info.start_fixed = false; + // pci.basic_info.dofs_fixed + + // Create Kinematic Object + pci.kin = pci.env->getManipulator(pci.basic_info.manip); + + // Populate Init Info + Eigen::VectorXd start_pos = pci.env->getCurrentJointValues(pci.kin->getName()); + + pci.init_info.type = InitInfo::STATIONARY; + pci.init_info.data = start_pos.transpose().replicate(pci.basic_info.n_steps, 1); + + // Populate Cost Info + std::shared_ptr jv = std::shared_ptr(new JointVelCostInfo); + jv->coeffs = std::vector(7, 5.0); + jv->name = "joint_vel"; + jv->term_type = TT_COST; + pci.cost_infos.push_back(jv); + + std::shared_ptr collision = std::shared_ptr(new CollisionCostInfo); + collision->name = "collision"; + collision->term_type = TT_COST; + collision->continuous = false; + collision->first_step = 0; + collision->last_step = pci.basic_info.n_steps - 1; + collision->gap = 1; + collision->info = createSafetyMarginDataVector(pci.basic_info.n_steps, 0.025, 20); + pci.cost_infos.push_back(collision); + + // Populate Constraints + double delta = 0.5 / pci.basic_info.n_steps; + for (auto i = 0; i < pci.basic_info.n_steps; ++i) + { + Vector3d xyz(0.5, -0.2 + delta * i, 0.62); + Vector4d wxyz(0.0, 0.0, 1.0, 0.0); + + Affine3d cur_pose; + cur_pose.translation() = xyz; + Quaterniond q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); + cur_pose.linear() = q.matrix(); + pose_inverses_.insert(pose_inverses_.begin() + i, cur_pose.inverse()); + + std::shared_ptr position(new StaticPoseCostInfo); + position->name = "position_" + std::to_string(i); + position->term_type = TT_CNT; + position->name = "waypoint_cart_" + std::to_string(i); + position->link = "tool0"; + position->timestep = i; + position->xyz = xyz; + position->wxyz = wxyz; + position->pos_coeffs = Eigen::Vector3d(10, 10, 10); + position->rot_coeffs = Eigen::Vector3d::Zero(); + pci.cnt_infos.push_back(position); + + if (constraint_type_ == "aligned") + { + std::shared_ptr aligned(new AlignedAxisTermInfo); + aligned->tolerance = tol_; + aligned->axis = Vector3d::UnitX(); + aligned->term_type = TT_CNT; + aligned->name = "aligned_" + std::to_string(i); + aligned->link = "tool0"; + aligned->timestep = i; + aligned->wxyz = Eigen::Vector4d(0.0, 0.0, 1.0, 0.0); + aligned->axis_coeff = 2.0; + aligned->angle_coeff = 10.0; + pci.cnt_infos.push_back(aligned); + } + else if (constraint_type_ == "conical") + { + std::shared_ptr conical(new ConicalAxisTermInfo); + conical->tolerance = tol_; + conical->axis = Vector3d::UnitZ(); + conical->term_type = TT_CNT; + conical->name = "conical_" + std::to_string(i); + conical->link = "tool0"; + conical->timestep = i; + conical->wxyz = Eigen::Vector4d(0.0, 0.0, 1.0, 0.0); + conical->weight = 10.0; + pci.cnt_infos.push_back(conical); + } + else + { + ROS_ERROR("%s is not a valid constraint type. Exiting", constraint_type_.c_str()); + exit(-1); + } + } + + return ConstructProblem(pci); + } + + virtual void SetUp() + { + // Initial setup + std::string urdf_xml_string, srdf_xml_string; + nh_.getParam(ROBOT_DESCRIPTION_PARAM, urdf_xml_string); + nh_.getParam(ROBOT_SEMANTIC_PARAM, srdf_xml_string); + urdf_model_ = urdf::parseURDF(urdf_xml_string); + + srdf_model_ = srdf::ModelSharedPtr(new srdf::Model); + srdf_model_->initString(*urdf_model_, srdf_xml_string); + env_ = tesseract_ros::KDLEnvPtr(new tesseract_ros::KDLEnv); + plotter_ = tesseract_ros::ROSBasicPlottingPtr(new tesseract_ros::ROSBasicPlotting(env_)); + assert(env_ != nullptr); + + bool success = env_->init(urdf_model_, srdf_model_); + assert(success); + + pcl::PointCloud full_cloud; + double delta = 0.05; + int length = (1 / delta); + + for (int x = 0; x < length; ++x) + for (int y = 0; y < length; ++y) + for (int z = 0; z < length; ++z) + full_cloud.push_back(pcl::PointXYZ(-0.5 + x * delta, -0.5 + y * delta, -0.5 + z * delta)); + + sensor_msgs::PointCloud2 pointcloud_msg; + pcl::toROSMsg(full_cloud, pointcloud_msg); + + octomap::Pointcloud octomap_data; + octomap::pointCloud2ToOctomap(pointcloud_msg, octomap_data); + octomap::OcTree* octree = new octomap::OcTree(2 * delta); + octree->insertPointCloud(octomap_data, octomap::point3d(0, 0, 0)); + + AttachableObjectPtr obj(new AttachableObject()); + shapes::OcTree* octomap_world = new shapes::OcTree(std::shared_ptr(octree)); + Eigen::Affine3d octomap_pose; + + octomap_pose.setIdentity(); + octomap_pose.translation() = Eigen::Vector3d(1, 0, 0); + + obj->name = "octomap_attached"; + obj->collision.shapes.push_back(shapes::ShapeConstPtr(octomap_world)); + obj->collision.shape_poses.push_back(octomap_pose); + obj->collision.collision_object_types.push_back(CollisionObjectType::UseShapeType); + env_->addAttachableObject(obj); + + AttachedBodyInfo attached_body; + attached_body.object_name = "octomap_attached"; + attached_body.parent_link_name = "base_link"; + attached_body.transform = Eigen::Affine3d::Identity(); + env_->attachBody(attached_body); + + // Set the robot initial state + std::unordered_map ipos; + ipos["joint_a1"] = -0.4; + ipos["joint_a2"] = 0.2762; + ipos["joint_a3"] = 0.0; + ipos["joint_a4"] = -1.3348; + ipos["joint_a5"] = 0.0; + ipos["joint_a6"] = 1.4959; + ipos["joint_a7"] = 0.0; + + env_->setState(ipos); + + plotter_->plotScene(); + + // Set Log Level + gLogLevel = util::LevelError; + } +}; + +TEST_P(AngularConstraintTest, AngularConstraint) +{ + // Setup Problem + TrajOptProbPtr prob; + testInfo info = GetParam(); + std::string method = info.method; + constraint_type_ = info.constraint; + + int num_iterations = method == "cpp" ? 3 : 1; + for (int i = 0; i < num_iterations; i++) + { + tol_ = (method == "cpp" ? 5.0 * (i + 1) : 5.0) * M_PI / 180.0; + ; + + if (method == "cpp") + { + prob = cppMethod(); + } + else if (method == "json") + { + prob = jsonMethod(); + } + else + { + ROS_ERROR("Method string invalid. Exiting"); + exit(-1); + } + + tesseract::ContactResultMap collisions; + const std::vector& joint_names = prob->GetKin()->getJointNames(); + const std::vector& link_names = prob->GetKin()->getLinkNames(); + + env_->continuousCollisionCheckTrajectory(joint_names, link_names, prob->GetInitTraj(), collisions); + + tesseract::ContactResultVector collision_vector; + tesseract::moveContactResultsMapToContactResultsVector(collisions, collision_vector); + ROS_INFO("Initial trajector number of continuous collisions: %lu\n", collision_vector.size()); + + BasicTrustRegionSQP opt(prob); + if (plotting_) + { + opt.addCallback(PlotCallback(*prob, plotter_)); + } + + opt.initialize(trajToDblVec(prob->GetInitTraj())); + ros::Time tStart = ros::Time::now(); + opt.optimize(); + ROS_INFO("planning time: %.3f", (ros::Time::now() - tStart).toSec()); + + if (plotting_) + { + plotter_->clear(); + } + + collisions.clear(); + env_->continuousCollisionCheckTrajectory(joint_names, link_names, getTraj(opt.x(), prob->GetVars()), collisions); + ROS_INFO("Final trajectory number of continuous collisions: %lu\n", collisions.size()); + + auto manip = prob->GetKin(); + auto change_base = env_->getLinkTransform(manip->getBaseLinkName()); + + Affine3d tcp; + tcp.setIdentity(); + + TrajArray traj = getTraj(opt.x(), prob->GetVars()); + for (auto j = 0; j < traj.rows(); j++) + { + VectorXd joint_angles(traj.cols()); + for (auto k = 0; k < traj.cols(); k++) + { + joint_angles(k) = traj(j, k); + } + + Affine3d pose; + manip->calcFwdKin(pose, change_base, joint_angles); + + Affine3d pose_inv = pose_inverses_.at(j); + Affine3d err = pose_inv * (pose * tcp); + + if (constraint_type_ == "aligned") + { + AngleAxisd aa(err.rotation()); + double aligned_angle = aa.angle(); + EXPECT_LE(aligned_angle, tol_ * (1.0 + approx_equals_diff_)); + + Vector3d axis = aa.axis(); + double dot_prod = axis.dot(Vector3d::UnitX()); + EXPECT_NEAR(1.0, fabs(dot_prod), approx_equals_diff_); + } + else + { // conical + Matrix3d orientation(err.rotation()); + double conical_angle = acos(orientation(2, 2)); + EXPECT_LE(conical_angle, tol_ * (1 + approx_equals_diff_)); + } + } + } +} + +INSTANTIATE_TEST_CASE_P(Tests, + AngularConstraintTest, + ::testing::Values(testInfo("json", "aligned"), + testInfo("json", "conical"), + testInfo("cpp", "aligned"), + testInfo("cpp", "conical"))); + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "angular_constraint_unit"); + ros::NodeHandle pnh("~"); + + // Get ROS Parameters + pnh.param("plotting", plotting_, plotting_); + + return RUN_ALL_TESTS(); +} diff --git a/trajopt/test/angular_constraint_unit.launch b/trajopt/test/angular_constraint_unit.launch new file mode 100644 index 00000000..72e2bc0f --- /dev/null +++ b/trajopt/test/angular_constraint_unit.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/trajopt/test/basic_cartesian_plan_aligned_axis.json b/trajopt/test/basic_cartesian_plan_aligned_axis.json new file mode 100644 index 00000000..ab122aab --- /dev/null +++ b/trajopt/test/basic_cartesian_plan_aligned_axis.json @@ -0,0 +1,169 @@ +{ + "basic_info" : + { + "n_steps" : 5, + "manip" : "manipulator", + "start_fixed" : false + }, + "costs" : + [ + { + "type" : "joint_vel", + "params": + { + "coeffs" : [5] + } + }, + { + "type" : "collision", + "params" : + { + "coeffs" : [20], + "dist_pen" : [0.025], + "continuous" : false + } + } + ], + "constraints" : + [ + { + "name" : "aligned_1", + "type" : "aligned_axis", + "params" : + { + "timestep" : 0, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "axis_coeff" : 2.0, + "angle_coeff" : 10.0, + "axis" : [1, 0, 0], + "tolerance" : 0.087266 + } + }, + { + "name" : "aligned_2", + "type" : "aligned_axis", + "params" : + { + "timestep" : 1, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "axis_coeff" : 2.0, + "angle_coeff" : 10.0, + "axis" : [1, 0, 0], + "tolerance" : 0.087266 + } + }, + { + "name" : "aligned_3", + "type" : "aligned_axis", + "params" : + { + "timestep" : 2, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "axis_coeff" : 2.0, + "angle_coeff" : 10.0, + "axis" : [1, 0, 0], + "tolerance" : 0.087266 + } + }, + { + "name" : "aligned_4", + "type" : "aligned_axis", + "params" : + { + "timestep" : 3, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "axis_coeff" : 2.0, + "angle_coeff" : 10.0, + "axis" : [1, 0, 0], + "tolerance" : 0.087266 + } + }, + { + "name" : "aligned_5", + "type" : "aligned_axis", + "params" : + { + "timestep" : 4, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "axis_coeff" : 2.0, + "angle_coeff" : 10.0, + "axis" : [1, 0, 0], + "tolerance" : 0.087266 + } + }, + { + "name" : "waypoint_cart_1", + "type" : "static_pose", + "params" : + { + "timestep" : 0, + "xyz" : [0.5, -0.2, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "rot_coeffs" : [0, 0, 0] + } + }, + { + "name" : "waypoint_cart_2", + "type" : "static_pose", + "params" : + { + "timestep" : 1, + "xyz" : [0.5, -0.1, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "rot_coeffs" : [0, 0, 0] + } + }, + { + "name" : "waypoint_cart_3", + "type" : "static_pose", + "params" : + { + "timestep" : 2, + "xyz" : [0.5, 0.0, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "rot_coeffs" : [0, 0, 0] + } + }, + { + "name" : "waypoint_cart_4", + "type" : "static_pose", + "params" : + { + "timestep" : 3, + "xyz" : [0.5, 0.1, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "rot_coeffs" : [0, 0, 0] + } + }, + { + "name" : "waypoint_cart_5", + "type" : "static_pose", + "params" : + { + "timestep" : 4, + "xyz" : [0.5, 0.2, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "rot_coeffs" : [0, 0, 0] + } + } + ], + "init_info" : + { + "type" : "stationary" + } +} diff --git a/trajopt/test/basic_cartesian_plan_conical_axis.json b/trajopt/test/basic_cartesian_plan_conical_axis.json new file mode 100644 index 00000000..18ad2c63 --- /dev/null +++ b/trajopt/test/basic_cartesian_plan_conical_axis.json @@ -0,0 +1,164 @@ +{ + "basic_info" : + { + "n_steps" : 5, + "manip" : "manipulator", + "start_fixed" : false + }, + "costs" : + [ + { + "type" : "joint_vel", + "params": + { + "coeffs" : [5] + } + }, + { + "type" : "collision", + "params" : + { + "coeffs" : [20], + "dist_pen" : [0.025], + "continuous" : false + } + } + ], + "constraints" : + [ + { + "name" : "conical_1", + "type" : "conical_axis", + "params" : + { + "timestep" : 0, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "weight" : 10.0, + "axis" : [0, 0, 1], + "tolerance" : 0.087266 + } + }, + { + "name" : "conical_2", + "type" : "conical_axis", + "params" : + { + "timestep" : 1, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "weight" : 10.0, + "axis" : [0, 0, 1], + "tolerance" : 0.087266 + } + }, + { + "name" : "conical_3", + "type" : "conical_axis", + "params" : + { + "timestep" : 2, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "weight" : 10.0, + "axis" : [0, 0, 1], + "tolerance" : 0.087266 + } + }, + { + "name" : "conical_4", + "type" : "conical_axis", + "params" : + { + "timestep" : 3, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "weight" : 10.0, + "axis" : [0, 0, 1], + "tolerance" : 0.087266 + } + }, + { + "name" : "conical_5", + "type" : "conical_axis", + "params" : + { + "timestep" : 4, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "weight" : 10.0, + "axis" : [0, 0, 1], + "tolerance" : 0.087266 + } + }, + { + "name" : "waypoint_cart_1", + "type" : "static_pose", + "params" : + { + "timestep" : 0, + "xyz" : [0.5, -0.2, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "rot_coeffs" : [0, 0, 0] + } + }, + { + "name" : "waypoint_cart_2", + "type" : "static_pose", + "params" : + { + "timestep" : 1, + "xyz" : [0.5, -0.1, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "rot_coeffs" : [0, 0, 0] + } + }, + { + "name" : "waypoint_cart_3", + "type" : "static_pose", + "params" : + { + "timestep" : 2, + "xyz" : [0.5, 0.0, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "rot_coeffs" : [0, 0, 0] + } + }, + { + "name" : "waypoint_cart_4", + "type" : "static_pose", + "params" : + { + "timestep" : 3, + "xyz" : [0.5, 0.1, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "rot_coeffs" : [0, 0, 0] + } + }, + { + "name" : "waypoint_cart_5", + "type" : "static_pose", + "params" : + { + "timestep" : 4, + "xyz" : [0.5, 0.2, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "rot_coeffs" : [0, 0, 0] + } + } + ], + "init_info" : + { + "type" : "stationary" + } +} diff --git a/trajopt/test/cast_cost_attached_unit.cpp b/trajopt/test/cast_cost_attached_unit.cpp index 4fc735ef..e88d0ae7 100644 --- a/trajopt/test/cast_cost_attached_unit.cpp +++ b/trajopt/test/cast_cost_attached_unit.cpp @@ -200,7 +200,6 @@ int main(int argc, char** argv) testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "trajopt_cast_cost_attached_unit"); ros::NodeHandle pnh("~"); - pnh.param("plotting", plotting, false); return RUN_ALL_TESTS(); } diff --git a/trajopt_examples/src/basic_cartesian_plan.cpp b/trajopt_examples/src/basic_cartesian_plan.cpp index 75ef88dd..b116ca51 100644 --- a/trajopt_examples/src/basic_cartesian_plan.cpp +++ b/trajopt_examples/src/basic_cartesian_plan.cpp @@ -110,6 +110,7 @@ TrajOptProbPtr cppMethod() collision->last_step = pci.basic_info.n_steps - 1; collision->gap = 1; collision->info = createSafetyMarginDataVector(pci.basic_info.n_steps, 0.025, 20); + pci.cost_infos.push_back(collision); // Populate Constraints double delta = 0.5 / pci.basic_info.n_steps; @@ -230,7 +231,7 @@ int main(int argc, char** argv) tesseract::ContactResultVector collision_vector; tesseract::moveContactResultsMapToContactResultsVector(collisions, collision_vector); - ROS_INFO("Initial trajector number of continuous collisions: %lui\n", collision_vector.size()); + ROS_INFO("Initial trajector number of continuous collisions: %lu\n", collision_vector.size()); BasicTrustRegionSQP opt(prob); if (plotting_) @@ -250,5 +251,5 @@ int main(int argc, char** argv) collisions.clear(); env_->continuousCollisionCheckTrajectory(joint_names, link_names, getTraj(opt.x(), prob->GetVars()), collisions); - ROS_INFO("Final trajectory number of continuous collisions: %lui\n", collisions.size()); + ROS_INFO("Final trajectory number of continuous collisions: %lu\n", collisions.size()); }