diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml index cf87b3fdd2..220e2af246 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml @@ -14,6 +14,7 @@ respawn="true" output="screen"> + diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/CMakeLists.txt b/jsk_pr2_robot/pr2_base_trajectory_action/CMakeLists.txt index 8102f62632..048aa8ed00 100644 --- a/jsk_pr2_robot/pr2_base_trajectory_action/CMakeLists.txt +++ b/jsk_pr2_robot/pr2_base_trajectory_action/CMakeLists.txt @@ -1,26 +1,45 @@ cmake_minimum_required(VERSION 2.8.3) project(pr2_base_trajectory_action) -find_package(catkin REQUIRED COMPONENTS roscpp actionlib geometry_msgs nav_msgs trajectory_msgs pr2_controllers_msgs actionlib_msgs pr2_mechanism_model angles) +find_package(catkin REQUIRED COMPONENTS + actionlib + angles + control_msgs + geometry_msgs + nav_msgs + roscpp + trajectory_msgs +) find_package(Boost REQUIRED COMPONENTS thread) -include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +include_directories(include + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS}) catkin_package( DEPENDS - CATKIN_DEPENDS roscpp actionlib geometry_msgs nav_msgs trajectory_msgs pr2_controllers_msgs actionlib_msgs pr2_mechanism_model angles - INCLUDE_DIRS # TODO include - LIBRARIES # TODO + CATKIN_DEPENDS actionlib control_msgs geometry_msgs nav_msgs + INCLUDE_DIRS include + LIBRARIES ) -add_executable(pr2_base_trajectory_action src/pr2_base_trajectory_action.cpp) +add_executable(pr2_base_trajectory_action + src/pr2_base_trajectory_action_controller.cpp + src/pr2_base_trajectory_action_controller_node.cpp) target_link_libraries(pr2_base_trajectory_action ${catkin_LIBRARIES} ${Boost_LIBRARIES}) install(DIRECTORY config include launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) install(TARGETS pr2_base_trajectory_action - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +if(CATKIN_ENABLE_TESTING) + find_package(rostest) + catkin_add_gtest(spline_test + test/spline_test.cpp) + target_link_libraries(spline_test ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +# add_rostest(test/pr2_base_trajectory_action.test) +endif() diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/include/pr2_base_trajectory_action/math_spline.h b/jsk_pr2_robot/pr2_base_trajectory_action/include/pr2_base_trajectory_action/math_spline.h new file mode 100644 index 0000000000..6778b7eb29 --- /dev/null +++ b/jsk_pr2_robot/pr2_base_trajectory_action/include/pr2_base_trajectory_action/math_spline.h @@ -0,0 +1,217 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * math_spline.h + * Author: Yuki Furuta + */ + +#ifndef MATH_SPLINE_H__ +#define MATH_SPLINE_H__ + +#include +#include +#include +#include +#include + +namespace pr2_base_trajectory_action { + + static inline void pows(int n, double base, double* ret) + { + ret[0] = 1.0; + for(int i = 1; i <= n; ++i) + { + ret[i] = ret[i-1] * base; + } + } + + struct Motion + { + double position, velocity, acceleration; + Motion() : position(0.0), velocity(DBL_MAX), acceleration(DBL_MAX) {} + Motion(double position) : position(position), velocity(DBL_MAX), acceleration(DBL_MAX) {} + Motion(double position, double velocity) + : position(position), velocity(velocity), acceleration(DBL_MAX) {} + Motion(double position, double velocity, double acceleration) + : position(position), velocity(velocity), acceleration(acceleration) {} + friend std::ostream &operator << (std::ostream &os, const Motion &m) + { + return os << ""; + } + }; + struct BaseMotion + { + Motion x, y, yaw; + const Motion &operator[] (size_t i) const { + switch (i) { + case 0: return x; + case 1: return y; + case 2: return yaw; + default: throw std::out_of_range("range must be 0-2"); + } + } + Motion &operator[] (size_t i) { + switch (i) { + case 0: return x; + case 1: return y; + case 2: return yaw; + default: throw std::out_of_range("range must be 0-2"); + } + } + friend std::ostream &operator << (std::ostream &os, const BaseMotion &m) { + return os << ""; + } + bool hasVelocity() const { + return x.velocity != DBL_MAX && y.velocity != DBL_MAX && yaw.velocity != DBL_MAX; + } + bool hasAcceleration() const { + return x.acceleration != DBL_MAX && y.acceleration != DBL_MAX && yaw.acceleration != DBL_MAX; + } + }; + + struct Spline + { + std::vector coefficients; + Spline() : coefficients(6, 0.0) {}; + virtual ~Spline(){ coefficients.clear(); }; + virtual void sample(const double &time, Motion &motion) const { + double t[6]; + pows(5, time, t); + motion.position = + t[0] * coefficients[0] + + t[1] * coefficients[1] + + t[2] * coefficients[2] + + t[3] * coefficients[3] + + t[4] * coefficients[4] + + t[5] * coefficients[5]; + + motion.velocity = + 1.0 * t[0] * coefficients[1] + + 2.0 * t[1] * coefficients[2] + + 3.0 * t[2] * coefficients[3] + + 4.0 * t[3] * coefficients[4] + + 5.0 * t[4] * coefficients[5]; + + motion.acceleration = + 2.0 * t[0] * coefficients[2] + + 6.0 * t[1] * coefficients[3] + + 12.0 * t[2] * coefficients[4] + + 20.0 * t[3] * coefficients[5]; + } + + virtual void sampleWithTimeBounds(const double &duration, + const double &time, Motion &motion) const { + if (time < 0.0) { + sample(0.0, motion); + motion.velocity = 0.0; + motion.acceleration = 0.0; + } else if (time > duration) { + sample(duration, motion); + motion.velocity = 0.0; + motion.acceleration = 0.0; + } else { + sample(time, motion); + } + } + }; + + struct Line : public Spline { + Line(const Motion &start, const Motion &end, const double &duration) { + coefficients[0] = start.position; + if (duration == 0.0) coefficients[1] = 0.0; + else coefficients[1] = (end.position - start.position) / duration; + } + }; + + struct CubicSpline : public Spline { + CubicSpline(const Motion &start, const Motion &end, const double &duration) { + if (duration == 0.0) { + coefficients[0] = end.position; + coefficients[1] = end.velocity; + } else { + double t[4]; + pows(3, duration, t); + coefficients[0] = start.position; + coefficients[1] = start.velocity; + coefficients[2] = (-3.0 * start.position + + 3.0 * end.position - + 2.0 * start.velocity * t[1] - + end.velocity * t[1]) / t[2]; + coefficients[3] = (2.0 * start.position - + 2.0 * end.position + + start.velocity * t[1] + + end.velocity * t[1]) / t[3]; + } + } + }; + + struct QuinticSpline : public Spline { + QuinticSpline(const Motion &start, const Motion &end, const double &duration) { + if (duration == 0.0) { + coefficients[0] = end.position; + coefficients[1] = end.velocity; + coefficients[2] = 0.5 * end.acceleration; + } else { + double t[6]; + pows(5, duration, t); + + coefficients[0] = start.position; + coefficients[1] = start.velocity; + coefficients[2] = 0.5 * start.acceleration; + coefficients[3] = (-20.0 * start.position + + 20.0 * end.position - + 3.0 * start.acceleration * t[2] + + end.acceleration * t[2] - + 12.0 * start.velocity * t[1] - + 8.0 * end.velocity * t[1]) / (2.0 * t[3]); + coefficients[4] = ( 30.0 * start.position - + 30.0 * end.position + + 3.0 * start.acceleration * t[2] - + 2.0 * end.acceleration * t[2] + + 16.0 * start.velocity * t[1] + + 14.0 * end.velocity * t[1]) / (2.0 * t[4]); + coefficients[5] = (-12.0 * start.position + + 12.0 * end.position - + start.acceleration * t[2] + + end.acceleration * t[2] - + 6.0 * start.velocity * t[1] - + 6.0 * end.velocity * t[1]) / (2.0*t[5]); + } + } + }; +} // namespace + +#endif // MATH_SPLINE_H__ diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/include/pr2_base_trajectory_action/pr2_base_trajectory_action.h b/jsk_pr2_robot/pr2_base_trajectory_action/include/pr2_base_trajectory_action/pr2_base_trajectory_action.h deleted file mode 100644 index 94c685dc30..0000000000 --- a/jsk_pr2_robot/pr2_base_trajectory_action/include/pr2_base_trajectory_action/pr2_base_trajectory_action.h +++ /dev/null @@ -1,144 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/*! - \author Stuart Glaser - - \class pr2_controller_interface::JointTrajectoryActionController - -*/ - -#ifndef JOINT_TRAJECTORY_ACTION_CONTROLLER_H__ -#define JOINT_TRAJECTORY_ACTION_CONTROLLER_H__ - -#include - -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -class BaseTrajectoryActionController -{ - typedef actionlib::ActionServer JTAS; - typedef JTAS::GoalHandle GoalHandle; - - // dummy class - class JointState { - public: - std::string name; - double position_, velocity_; - double max_abs_velocity_; - double commanded_velocity_; - double commanded_effort_;// same unit as velocity - }; - -public: - - BaseTrajectoryActionController(const ros::NodeHandle& n); - ~BaseTrajectoryActionController(); - - bool init(); - - void starting(); - void update(); - -private: - bool use_pid; - JointState x_joint_,y_joint_,z_joint_; - ros::Time last_time_, robot_time_; - std::vector joints_; - double goal_time_constraint_; - double goal_threshold_; - double stopped_velocity_tolerance_; - std::vector goal_constraints_; - std::vector trajectory_constraints_; - - ros::NodeHandle node_; - - void commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg); - ros::Publisher pub_command_; - - void odomCB(const nav_msgs::Odometry::ConstPtr& msg); - ros::Subscriber sub_odom_; - - boost::scoped_ptr action_server_; - void goalCB(GoalHandle gh); - void cancelCB(GoalHandle gh); - ros::Timer goal_handle_timer_; - - boost::shared_ptr active_goal_; - - void commandTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &traj, boost::shared_ptr gh = boost::shared_ptr((GoalHandle*)NULL)); - - // coef[0] + coef[1]*t + ... + coef[5]*t^5 - struct Spline - { - std::vector coef; - - Spline() : coef(6, 0.0) {} - }; - - struct Segment - { - double start_time; - double duration; - std::vector splines; - - boost::shared_ptr gh; - }; - typedef std::vector SpecifiedTrajectory; - - boost::shared_ptr current_trajectory_; - - // Holds the trajectory that we are currently following. The mutex - // guarding current_trajectory_ is locked from within realtime, so - // it may only be locked for a bounded duration. - //boost::shared_ptr current_trajectory_; - //boost::recursive_mutex current_trajectory_lock_RT_; - - std::vector q, qd, qdd; // Preallocated in init - std::vector e, ed, edd; // Preallocated in init - - // Samples, but handling time bounds. When the time is past the end - // of the spline duration, the position is the last valid position, - // and the derivatives are all 0. - static void sampleSplineWithTimeBounds(const std::vector& coefficients, double duration, double time, - double& position, double& velocity, double& acceleration); -}; - - -#endif diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/include/pr2_base_trajectory_action/pr2_base_trajectory_action_controller.h b/jsk_pr2_robot/pr2_base_trajectory_action/include/pr2_base_trajectory_action/pr2_base_trajectory_action_controller.h new file mode 100644 index 0000000000..2bea0be501 --- /dev/null +++ b/jsk_pr2_robot/pr2_base_trajectory_action/include/pr2_base_trajectory_action/pr2_base_trajectory_action_controller.h @@ -0,0 +1,215 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * pr2_base_trajectory_action_controller.h + * Author: Yuki Furuta + */ + +#ifndef PR2_BASE_TRAJECTORY_ACTION_CONTROLLER_H__ +#define PR2_BASE_TRAJECTORY_ACTION_CONTROLLER_H__ + +#include +#include +#include +//#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace pr2_base_trajectory_action { + typedef actionlib::ActionServer Server; + typedef Server::GoalHandle GoalHandle; + + struct JointState { + std::string name; + double position, velocity; + double max_velocity; + double commanded_velocity; + double commanded_effort; + }; + + struct BaseJointState { + ros::Time real_latest_time; + JointState x, y, yaw; + const JointState &operator[] (size_t i) const { + if (i == 0) return x; + else if (i == 1) return y; + else if (i == 2) return yaw; + else throw std::out_of_range("range must be 0-2"); + } + JointState &operator[] (size_t i) { + if (i == 0) return x; + else if (i == 1) return y; + else if (i == 2) return yaw; + else throw std::out_of_range("range must be 0-2"); + } + }; + + struct Trajectory { + struct Segment + { + double start_time; + double duration; + Spline splines[3]; + boost::shared_ptr gh; + Segment(){} + Segment(const double &start_time, + const double &duration, + const boost::shared_ptr &gh, + const Spline &spline_x, + const Spline &spline_y, + const Spline &spline_yaw) : + start_time(start_time), + duration(duration), + gh(gh) { + splines[0] = spline_x; + splines[1] = spline_y; + splines[2] = spline_yaw; + } + void sample(const double &time, BaseMotion &base) { + for (int i = 0; i < 3; ++i) + splines[i].sampleWithTimeBounds(duration, time, base[i]); + } + typedef boost::shared_ptr< ::pr2_base_trajectory_action::Trajectory::Segment > Ptr; + }; + std::vector segments; + + Trajectory() {} + Trajectory(int size) : segments(size) {} + + bool nextSegment(const double &time, Segment &seg) { + if (size() == 0) { + ROS_ERROR("no segments in trajectory"); + return false; + } + int seg_idx = -1; + while (seg_idx + 1 < size() && + segments[seg_idx+1].start_time < time) + ++seg_idx; + if (seg_idx == -1) { + ROS_ERROR("No earlier segments. Segment starts at %.3lf (now: %.3lf)", + segments[0].start_time, time); + return false; + } + seg = segments[seg_idx]; + return true; + } + bool motionAtTime(const double &time, BaseMotion &base) { + Segment s; + if (!nextSegment(time, s)) return false; + double diff_time = time - s.start_time; + s.sample(diff_time, base); + return true; + } + bool goalAtTime(const double &time, boost::shared_ptr &gh) { + Segment s; + if (!nextSegment(time, s)) return false; + gh = s.gh; + return true; + } + double endTime() { + Segment s = segments[size() - 1]; + return s.start_time + s.duration; + } + inline int size() const { + return segments.size(); + } + typedef boost::shared_ptr< ::pr2_base_trajectory_action::Trajectory > Ptr; + typedef boost::shared_ptr< ::pr2_base_trajectory_action::Trajectory const > ConstPtr; + }; + + class Controller + { + + boost::mutex mutex_; + ros::NodeHandle nh_, pnh_; + ros::Subscriber odom_sub_; + ros::Publisher cmd_vel_pub_; + ros::Timer update_timer_; + boost::shared_ptr as_; + boost::shared_ptr active_goal_; + Trajectory::Ptr active_traj_; + ros::Time real_latest_time_; + BaseJointState joints_; + double frequency_; + double stopped_velocity_tolerance_; + double goal_threshold_; + double goal_time_constraint_; + bool use_pid; + control_msgs::FollowJointTrajectoryFeedback::Ptr feedback_msg_; + geometry_msgs::Twist::Ptr cmd_vel_msg_; + + void setFeedback(const ros::Time &time, const BaseMotion &desired, const BaseMotion &error); + void clearTrajectory(); + void setTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &msg, + boost::shared_ptr &gh); + public: + + Controller(); + virtual ~Controller(); + void goalCallback(GoalHandle gh); + void cancelCallback(GoalHandle gh); + void odomCallback(const nav_msgs::Odometry::ConstPtr & msg); + void update(const ros::TimerEvent& event); + }; // class + + template + inline int findIndex(std::vector vec, T val) + { + typename + std::vector::iterator it = std::find(vec.begin(), vec.end(), val); + int d = it - vec.begin(); + return d == vec.size() ? -1 : d; + } + + template + inline boost::shared_ptr share_member(boost::shared_ptr e, M &m) + { + actionlib::EnclosureDeleter d(e); + boost::shared_ptr p(&m, d); + return p; + } +} // namespace + + +#endif // PR2_BASE_TRAJECTORY_ACTION_CONTROLLER_H__ diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/launch/pr2_base_trajectory_action.launch b/jsk_pr2_robot/pr2_base_trajectory_action/launch/pr2_base_trajectory_action.launch index c245c0be25..a4a3edbb32 100644 --- a/jsk_pr2_robot/pr2_base_trajectory_action/launch/pr2_base_trajectory_action.launch +++ b/jsk_pr2_robot/pr2_base_trajectory_action/launch/pr2_base_trajectory_action.launch @@ -1,10 +1,10 @@ - - - - - - - + + + + + + + diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/launch/pr2_base_trajectory_action_safe.launch b/jsk_pr2_robot/pr2_base_trajectory_action/launch/pr2_base_trajectory_action_safe.launch deleted file mode 100644 index 7f5535a4f8..0000000000 --- a/jsk_pr2_robot/pr2_base_trajectory_action/launch/pr2_base_trajectory_action_safe.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/mainpage.dox b/jsk_pr2_robot/pr2_base_trajectory_action/mainpage.dox deleted file mode 100644 index d5476b0bea..0000000000 --- a/jsk_pr2_robot/pr2_base_trajectory_action/mainpage.dox +++ /dev/null @@ -1,26 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b pr2_base_trajectory_action is ... - - - - -\section codeapi Code API - - - - -*/ diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/package.xml b/jsk_pr2_robot/pr2_base_trajectory_action/package.xml index 6658d57751..c434c1ac3c 100644 --- a/jsk_pr2_robot/pr2_base_trajectory_action/package.xml +++ b/jsk_pr2_robot/pr2_base_trajectory_action/package.xml @@ -3,53 +3,36 @@ 1.0.6 pr2_base_trajectory_action is a node that exposes and action interface to move robot base along a trajectory. - saito + Kei Okada + Yuki Furuta BSD http://ros.org/wiki/pr2_base_trajectory_action - saito + Yuki Furuta catkin - roscpp actionlib + angles + control_msgs geometry_msgs nav_msgs + roscpp trajectory_msgs - pr2_controllers_msgs - actionlib_msgs - pr2_mechanism_model - angles - roscpp actionlib + control_msgs geometry_msgs nav_msgs - trajectory_msgs - pr2_controllers_msgs - actionlib_msgs - pr2_mechanism_model - angles - - - - - - - - - - - + rostest - diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/src/pr2_base_trajectory_action.cpp b/jsk_pr2_robot/pr2_base_trajectory_action/src/pr2_base_trajectory_action.cpp deleted file mode 100644 index 067342219f..0000000000 --- a/jsk_pr2_robot/pr2_base_trajectory_action/src/pr2_base_trajectory_action.cpp +++ /dev/null @@ -1,763 +0,0 @@ -/* - * Copyright (c) 2011, JSK lab. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of JSK, university of Tokyo, nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -// Author: Manabu Saito - -#include -#include - -#include -#include -#include -#include -#include - -#include -#include "angles/angles.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "base_trajectory_action_node"); - ros::NodeHandle node;//("~"); - BaseTrajectoryActionController ac(node); - - ros::Rate r(40.0); - while(ros::ok()) { - ros::spinOnce(); - ac.update(); - r.sleep(); - } - - return 0; -} - -static inline void generatePowers(int n, double x, double* powers) -{ - powers[0] = 1.0; - for (int i=1; i<=n; i++) - { - powers[i] = powers[i-1]*x; - } -} - -static void getQuinticSplineCoefficients(double start_pos, double start_vel, double start_acc, - double end_pos, double end_vel, double end_acc, double time, std::vector& coefficients) -{ - coefficients.resize(6); - - if (time == 0.0) - { - coefficients[0] = end_pos; - coefficients[1] = end_vel; - coefficients[2] = 0.5*end_acc; - coefficients[3] = 0.0; - coefficients[4] = 0.0; - coefficients[5] = 0.0; - } - else - { - double T[6]; - generatePowers(5, time, T); - - coefficients[0] = start_pos; - coefficients[1] = start_vel; - coefficients[2] = 0.5*start_acc; - coefficients[3] = (-20.0*start_pos + 20.0*end_pos - 3.0*start_acc*T[2] + end_acc*T[2] - - 12.0*start_vel*T[1] - 8.0*end_vel*T[1]) / (2.0*T[3]); - coefficients[4] = (30.0*start_pos - 30.0*end_pos + 3.0*start_acc*T[2] - 2.0*end_acc*T[2] + - 16.0*start_vel*T[1] + 14.0*end_vel*T[1]) / (2.0*T[4]); - coefficients[5] = (-12.0*start_pos + 12.0*end_pos - start_acc*T[2] + end_acc*T[2] - - 6.0*start_vel*T[1] - 6.0*end_vel*T[1]) / (2.0*T[5]); - } -} - -/** - * \brief Samples a quintic spline segment at a particular time - */ -static void sampleQuinticSpline(const std::vector& coefficients, double time, - double& position, double& velocity, double& acceleration) -{ - // create powers of time: - double t[6]; - generatePowers(5, time, t); - - position = t[0]*coefficients[0] + - t[1]*coefficients[1] + - t[2]*coefficients[2] + - t[3]*coefficients[3] + - t[4]*coefficients[4] + - t[5]*coefficients[5]; - - velocity = t[0]*coefficients[1] + - 2.0*t[1]*coefficients[2] + - 3.0*t[2]*coefficients[3] + - 4.0*t[3]*coefficients[4] + - 5.0*t[4]*coefficients[5]; - - acceleration = 2.0*t[0]*coefficients[2] + - 6.0*t[1]*coefficients[3] + - 12.0*t[2]*coefficients[4] + - 20.0*t[3]*coefficients[5]; -} - -static void getCubicSplineCoefficients(double start_pos, double start_vel, - double end_pos, double end_vel, double time, std::vector& coefficients) -{ - coefficients.resize(4); - - if (time == 0.0) - { - coefficients[0] = end_pos; - coefficients[1] = end_vel; - coefficients[2] = 0.0; - coefficients[3] = 0.0; - } - else - { - double T[4]; - generatePowers(3, time, T); - - coefficients[0] = start_pos; - coefficients[1] = start_vel; - coefficients[2] = (-3.0*start_pos + 3.0*end_pos - 2.0*start_vel*T[1] - end_vel*T[1]) / T[2]; - coefficients[3] = (2.0*start_pos - 2.0*end_pos + start_vel*T[1] + end_vel*T[1]) / T[3]; - } -} - - -BaseTrajectoryActionController::BaseTrajectoryActionController(const ros::NodeHandle &nh) - : node_(nh) -{ - joints_.push_back(&x_joint_); - joints_.push_back(&y_joint_); - joints_.push_back(&z_joint_); - - init(); -} - -BaseTrajectoryActionController::~BaseTrajectoryActionController() -{ - pub_command_.shutdown(); - sub_odom_.shutdown(); - action_server_.reset(); -} - -bool BaseTrajectoryActionController::init() -{ - using namespace XmlRpc; - - // virtual joint settings - std::string ns = std::string("joint_trajectory_action"); - node_.param(ns+"/x_joint_name", x_joint_.name, std::string("base_link_x")); - node_.param(ns+"/y_joint_name", y_joint_.name, std::string("base_link_y")); - node_.param(ns+"/rotational_joint_name", z_joint_.name, - std::string("base_link_pan")); - - node_.param(ns+"/use_pid", use_pid, true); - node_.param(ns+"/goal_threshold", goal_threshold_, 0.01); - node_.param(ns+"/stopped_velocity_tolerance", stopped_velocity_tolerance_, 0.01); - node_.param(ns+"/goal_time_constraint", goal_time_constraint_, 0.0); - - for (size_t i = 0; i < joints_.size(); ++i) - { - node_.param(ns + "/" + joints_[i]->name + "/max_velocity", - joints_[i]->max_abs_velocity_, 0.2); - ROS_INFO("%s %f", ns.c_str(), joints_[i]->max_abs_velocity_); - } - - // Creates a dummy trajectory - boost::shared_ptr traj_ptr(new SpecifiedTrajectory(1)); - SpecifiedTrajectory &traj = *traj_ptr; - traj[0].start_time = ros::Time::now().toSec(); - traj[0].duration = 0.0; - traj[0].splines.resize(joints_.size()); - for (size_t j = 0; j < joints_.size(); ++j) - traj[0].splines[j].coef[0] = 0.0; - current_trajectory_ = traj_ptr; - - pub_command_ = node_.advertise("command", 10); - sub_odom_ = node_.subscribe("odom", 1, boost::bind(&BaseTrajectoryActionController::odomCB, this, _1)); - - q.resize(joints_.size()); - qd.resize(joints_.size()); - qdd.resize(joints_.size()); - e.resize(joints_.size()); - ed.resize(joints_.size()); - edd.resize(joints_.size()); - - action_server_.reset(new JTAS(node_, "joint_trajectory_action", - boost::bind(&BaseTrajectoryActionController::goalCB, this, _1), - boost::bind(&BaseTrajectoryActionController::cancelCB, this, _1))); - - ROS_DEBUG("base traj controller initialized;"); - - return true; -} - -void BaseTrajectoryActionController::starting() -{ - last_time_ = ros::Time::now(); - - // Creates a "hold current position" trajectory. - boost::shared_ptr hold_ptr(new SpecifiedTrajectory(1)); - SpecifiedTrajectory &hold = *hold_ptr; - hold[0].start_time = last_time_.toSec() - 0.001; - hold[0].duration = 0.0; - hold[0].splines.resize(joints_.size()); - for (size_t j = 0; j < joints_.size(); ++j) - hold[0].splines[j].coef[0] = joints_[j]->position_; - - current_trajectory_ = hold_ptr; -} - -void BaseTrajectoryActionController::update() -{ - ros::Time time = ros::Time::now();// + ros::Duration(0.05); - ros::Duration dt = time - last_time_; - last_time_ = time; - - if(active_goal_ == NULL) return; - - boost::shared_ptr traj_ptr; - traj_ptr = current_trajectory_; - if (!traj_ptr) - ROS_FATAL("The current trajectory can never be null"); - - // Only because this is what the code originally looked like. - const SpecifiedTrajectory &traj = *traj_ptr; - - // ------ Finds the current segment - - // Determines which segment of the trajectory to use. (Not particularly realtime friendly). - int seg = -1; - while (seg + 1 < (int)traj.size() && - traj[seg+1].start_time < time.toSec()) - { - ++seg; - } - - if (seg == -1) - { - if (traj.size() == 0) - ROS_ERROR("No segments in the trajectory"); - else - ROS_ERROR("No earlier segments. First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec()); - return; - } - - // ------ Trajectory Sampling - - for (size_t i = 0; i < q.size(); ++i) - { - sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration, - time.toSec() - traj[seg].start_time, - q[i], qd[i], qdd[i]); - } - - // Defference between odometry and sample point - std::vector error(joints_.size()); - for (size_t i = 0; i < joints_.size(); ++i) - { - // no odometry info - if(robot_time_ == ros::Time(0)) continue; - - sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration, - robot_time_.toSec() - traj[seg].start_time, - e[i], ed[i], edd[i]); - if(joints_[i] == &z_joint_) { - error[i] = angles::shortest_angular_distance(joints_[i]->position_, e[i]); - } else { - error[i] = e[i] - joints_[i]->position_; - } - - joints_[i]->commanded_effort_ = error[i] * 1.0; // P_gain:1.0 - } - - - // ------ Determines if the goal has failed or succeeded - - if (traj[seg].gh && traj[seg].gh == active_goal_) - { - ros::Time end_time(traj[seg].start_time + traj[seg].duration); - ROS_DEBUG("time: %f -> end: %f", time.toSec(), end_time.toSec()); - ROS_DEBUG("seg = %d, tarj.size() = %ld", seg, traj.size()); - if (time <= end_time) - { - // Verifies trajectory constraints - for (size_t j = 0; j < joints_.size(); ++j) - { - } - } - else if (seg == (int)traj.size() - 1) - { - // Checks that we have ended inside the goal constraints - bool inside_goal_constraints = true; - for (size_t i = 0; i < joints_.size() && inside_goal_constraints; ++i) - { - // It's important to be stopped if that's desired. - if (fabs(qd[i]) < 1e-6) - { - ROS_DEBUG("qd[%ld] = %f", i, qd[i]); - if (fabs(joints_[i]->velocity_) > stopped_velocity_tolerance_) { - ROS_WARN("fabs(joints_[%ld]->velocity_) = %f > %f (stopped_velocity_tolerance_)", - i, joints_[i]->velocity_, stopped_velocity_tolerance_); - inside_goal_constraints = false; - } - } - } - - for (size_t i = 0; i < joints_.size() && inside_goal_constraints; ++i) - { - if(fabs(error[i]) > goal_threshold_) { - ROS_WARN("fabs(error[%ld]) = %f > %f (goal_threshold)", - i, fabs(error[i]), goal_threshold_); - inside_goal_constraints = false; - } - } - - if (inside_goal_constraints) - { - traj[seg].gh->setSucceeded(); - active_goal_ = boost::shared_ptr((GoalHandle*)NULL); - ROS_DEBUG("base_traj success"); - //ROS_ERROR("Success! (%s)", traj[seg].gh->gh_.getGoalID().id.c_str()); - } - else if (time < end_time + ros::Duration(goal_time_constraint_)) - { - // Still have some time left to make it. - } - else - { - //ROS_WARN("Aborting because we wound up outside the goal constraints"); - traj[seg].gh->setAborted(); - active_goal_ = boost::shared_ptr((GoalHandle*)NULL); - ROS_WARN("base_traj aborted"); - } - } - - // check if new command violates the max velocity - for (size_t j = 0; j < joints_.size(); ++j) { - joints_[j]->commanded_velocity_ = qd[j] + (use_pid ? joints_[j]->commanded_effort_ : 0); - if(joints_[j]->max_abs_velocity_ < fabs(joints_[j]->commanded_velocity_)) - { - joints_[j]->commanded_velocity_ *= - joints_[j]->max_abs_velocity_/fabs(joints_[j]->commanded_velocity_); - ROS_WARN("joint(%s) violates its velocity limit.", joints_[j]->name.c_str()); - } - } - - // publish controll velocity - geometry_msgs::Twist vel; - double vx,vy,theta; // in odometry space - vx = joints_[0]->commanded_velocity_; - vy = joints_[1]->commanded_velocity_; - theta = joints_[2]->position_; - vel.linear.x = vx * cos(theta) + vy * sin(theta); - vel.linear.y = vy * cos(theta) - vx * sin(theta); - vel.angular.z = joints_[2]->commanded_velocity_; - pub_command_.publish(vel); - - ROS_DEBUG("pos = %f %f %f", joints_[0]->position_,joints_[1]->position_,joints_[2]->position_); - ROS_DEBUG("vel = %f %f %f", vel.linear.x, vel.linear.y, vel.angular.z); - } - -} - -void BaseTrajectoryActionController::commandTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &msg, boost::shared_ptr gh) -{ - ros::Time time = last_time_ + ros::Duration(0.01); - ROS_DEBUG("Figuring out new trajectory at %.3lf, with data from %.3lf", - time.toSec(), msg->header.stamp.toSec()); - - boost::shared_ptr new_traj_ptr(new SpecifiedTrajectory); - SpecifiedTrajectory &new_traj = *new_traj_ptr; - - // ------ If requested, performs a stop - - if (msg->points.empty()) - { - starting(); - return; - } - - // ------ Correlates the joints we're commanding to the joints in the message - - std::vector lookup(joints_.size(), -1); // Maps from an index in joints_ to an index in the msg - for (size_t j = 0; j < joints_.size(); ++j) - { - for (size_t k = 0; k < msg->joint_names.size(); ++k) - { - if (msg->joint_names[k] == joints_[j]->name) - { - lookup[j] = k; - break; - } - } - - if (lookup[j] == -1) - { - ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joints_[j]->name.c_str()); - return; - } - } - - // ------ Grabs the trajectory that we're currently following. - - boost::shared_ptr prev_traj_ptr; - prev_traj_ptr = current_trajectory_; - if (!prev_traj_ptr) - { - ROS_FATAL("The current trajectory can never be null"); - return; - } - const SpecifiedTrajectory &prev_traj = *prev_traj_ptr; - - // ------ Copies over the segments from the previous trajectory that are still useful. - - // Useful segments are still relevant after the current time. - int first_useful = -1; - while (first_useful + 1 < (int)prev_traj.size() && - prev_traj[first_useful + 1].start_time <= time.toSec()) - { - ++first_useful; - } - - // Useful segments are not going to be completely overwritten by the message's splines. - int last_useful = -1; - double msg_start_time; - if (msg->header.stamp == ros::Time(0.0)) - msg_start_time = time.toSec(); - else - msg_start_time = msg->header.stamp.toSec(); - /* - if (msg->points.size() > 0) - msg_start_time += msg->points[0].time_from_start.toSec(); - */ - - while (last_useful + 1 < (int)prev_traj.size() && - prev_traj[last_useful + 1].start_time < msg_start_time) - { - ++last_useful; - } - - if (last_useful < first_useful) - first_useful = last_useful; - - // Copies over the old segments that were determined to be useful. - for (int i = std::max(first_useful,0); i <= last_useful; ++i) - { - new_traj.push_back(prev_traj[i]); - } - - // We always save the last segment so that we know where to stop if - // there are no new segments. - if (new_traj.size() == 0) - new_traj.push_back(prev_traj[prev_traj.size() - 1]); - - // ------ Determines when and where the new segments start - - // Finds the end conditions of the final segment - Segment &last = new_traj[new_traj.size() - 1]; - std::vector prev_positions(joints_.size()); - std::vector prev_velocities(joints_.size()); - std::vector prev_accelerations(joints_.size()); - - double t = (msg->header.stamp == ros::Time(0.0) ? time.toSec() : msg->header.stamp.toSec()) - - last.start_time; - ROS_DEBUG("Initial conditions at %.3f for new set of splines:", t); - for (size_t i = 0; i < joints_.size(); ++i) - { - sampleSplineWithTimeBounds(last.splines[i].coef, last.duration, - t, - prev_positions[i], prev_velocities[i], prev_accelerations[i]); - ROS_DEBUG(" %.2lf, %.2lf, %.2lf (%s)", prev_positions[i], prev_velocities[i], - prev_accelerations[i], joints_[i]->name.c_str()); - } - - // ------ Tacks on the new segments - - std::vector positions; - std::vector velocities; - std::vector accelerations; - - std::vector durations(msg->points.size()); - if (msg->points.size() > 0) - durations[0] = msg->points[0].time_from_start.toSec(); - for (size_t i = 1; i < msg->points.size(); ++i) - durations[i] = (msg->points[i].time_from_start - msg->points[i-1].time_from_start).toSec(); - - // Checks if we should wrap - std::vector wrap(joints_.size(), 0.0); - if (msg->points[0].positions.empty()) { - ROS_ERROR("First point of trajectory has no positions"); - return; - } - ROS_DEBUG("wrap:"); - for (size_t j = 0; j < joints_.size(); ++j) - { - if (joints_[j] == &z_joint_) // z_joint_ angle is -pi <-> pi [rad] - { - double dist = angles::shortest_angular_distance(prev_positions[j], msg->points[0].positions[lookup[j]]); - wrap[j] = (prev_positions[j] + dist) - msg->points[0].positions[lookup[j]]; - ROS_DEBUG(" %.2lf - %s bc dist(%.2lf, %.2lf) = %.2lf", wrap[j], joints_[j]->name.c_str(), - prev_positions[j], msg->points[0].positions[lookup[j]], dist); - } - } - - for (size_t i = 0; i < msg->points.size(); ++i) - { - Segment seg; - - if (msg->header.stamp == ros::Time(0.0)) - seg.start_time = (time + msg->points[i].time_from_start).toSec() - durations[i]; - else - seg.start_time = (msg->header.stamp + msg->points[i].time_from_start).toSec() - durations[i]; - seg.duration = durations[i]; - seg.gh = gh; - seg.splines.resize(joints_.size()); - - // Checks that the incoming segment has the right number of elements. - - if (msg->points[i].accelerations.size() != 0 && msg->points[i].accelerations.size() != joints_.size()) - { - ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)msg->points[i].accelerations.size()); - return; - } - if (msg->points[i].velocities.size() != 0 && msg->points[i].velocities.size() != joints_.size()) - { - ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)msg->points[i].velocities.size()); - return; - } - if (msg->points[i].positions.size() != joints_.size()) - { - ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)msg->points[i].positions.size()); - return; - } - - // Re-orders the joints in the command to match the internal joint order. - - accelerations.resize(msg->points[i].accelerations.size()); - velocities.resize(msg->points[i].velocities.size()); - positions.resize(msg->points[i].positions.size()); - for (size_t j = 0; j < joints_.size(); ++j) - { - if (!accelerations.empty()) accelerations[j] = msg->points[i].accelerations[lookup[j]]; - if (!velocities.empty()) velocities[j] = msg->points[i].velocities[lookup[j]]; - if (!positions.empty()) positions[j] = msg->points[i].positions[lookup[j]] + wrap[j]; - } - - // Converts the boundary conditions to splines. - - for (size_t j = 0; j < joints_.size(); ++j) - { - if (prev_accelerations.size() > 0 && accelerations.size() > 0) - { - getQuinticSplineCoefficients( - prev_positions[j], prev_velocities[j], prev_accelerations[j], - positions[j], velocities[j], accelerations[j], - durations[i], - seg.splines[j].coef); - } - else if (prev_velocities.size() > 0 && velocities.size() > 0) - { - getCubicSplineCoefficients( - prev_positions[j], prev_velocities[j], - positions[j], velocities[j], - durations[i], - seg.splines[j].coef); - seg.splines[j].coef.resize(6, 0.0); - } - else - { - seg.splines[j].coef[0] = prev_positions[j]; - if (durations[i] == 0.0) - seg.splines[j].coef[1] = 0.0; - else - seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i]; - seg.splines[j].coef[2] = 0.0; - seg.splines[j].coef[3] = 0.0; - seg.splines[j].coef[4] = 0.0; - seg.splines[j].coef[5] = 0.0; - } - } - - // Pushes the splines onto the end of the new trajectory. - - new_traj.push_back(seg); - - // Computes the starting conditions for the next segment - - prev_positions = positions; - prev_velocities = velocities; - prev_accelerations = accelerations; - } - - //ROS_ERROR("Last segment goal id: %s", new_traj[new_traj.size()-1].gh->gh_.getGoalID().id.c_str()); - - // ------ Commits the new trajectory - - if (!new_traj_ptr) - { - ROS_ERROR("The new trajectory was null!"); - return; - } - - current_trajectory_ = new_traj_ptr; - ROS_DEBUG("The new trajectory has %d segments", (int)new_traj.size()); -#if 0 - for (size_t i = 0; i < std::min((size_t)20,new_traj.size()); ++i) - { - ROS_DEBUG("Segment %2ld: %.3lf for %.3lf", i, new_traj[i].start_time, new_traj[i].duration); - for (size_t j = 0; j < new_traj[i].splines.size(); ++j) - { - ROS_DEBUG(" %.2lf %.2lf %.2lf %.2lf , %.2lf %.2lf(%s)", - new_traj[i].splines[j].coef[0], - new_traj[i].splines[j].coef[1], - new_traj[i].splines[j].coef[2], - new_traj[i].splines[j].coef[3], - new_traj[i].splines[j].coef[4], - new_traj[i].splines[j].coef[5], - joints_[j]->name.c_str()); - } - } -#endif -} - -void BaseTrajectoryActionController::odomCB(const nav_msgs::Odometry::ConstPtr& msg) -{ - robot_time_ = msg->header.stamp; - x_joint_.position_ = msg->pose.pose.position.x; - y_joint_.position_ = msg->pose.pose.position.y; - z_joint_.position_ = atan2(msg->pose.pose.orientation.z, - msg->pose.pose.orientation.w) * 2; - //x_joint_.velocity_ = msg->twist.twist.linear.x; - //y_joint_.velocity_ = msg->twist.twist.linear.y; - //z_joint_.velocity_ = msg->twist.twist.angular.z; -} - -void BaseTrajectoryActionController::sampleSplineWithTimeBounds( - const std::vector& coefficients, double duration, double time, - double& position, double& velocity, double& acceleration) -{ - if (time < 0) - { - double _; - sampleQuinticSpline(coefficients, 0.0, position, _, _); - velocity = 0; - acceleration = 0; - } - else if (time > duration) - { - double _; - sampleQuinticSpline(coefficients, duration, position, _, _); - velocity = 0; - acceleration = 0; - } - else - { - sampleQuinticSpline(coefficients, time, - position, velocity, acceleration); - } -} - -static bool setsEqual(const std::vector &a, const std::vector &b) -{ - if (a.size() != b.size()) - return false; - - for (size_t i = 0; i < a.size(); ++i) - { - if (count(b.begin(), b.end(), a[i]) != 1) - return false; - } - for (size_t i = 0; i < b.size(); ++i) - { - if (count(a.begin(), a.end(), b[i]) != 1) - return false; - } - - return true; -} - -template -static boost::shared_ptr share_member(boost::shared_ptr enclosure, Member &member) -{ - actionlib::EnclosureDeleter d(enclosure); - boost::shared_ptr p(&member, d); - return p; -} - - -void BaseTrajectoryActionController::goalCB(GoalHandle gh) -{ - ROS_DEBUG("goalCB"); - - std::vector joint_names(joints_.size()); - for (size_t j = 0; j < joints_.size(); ++j) - joint_names[j] = joints_[j]->name; - - // Ensures that the joints in the goal match the joints we are commanding. - if (!setsEqual(joint_names, gh.getGoal()->trajectory.joint_names)) - { - ROS_ERROR("Joints on incoming goal don't match our joints"); - gh.setRejected(); - return; - } - - // Cancels the currently active goal. - if (active_goal_) - { - // Marks the current goal as canceled. - active_goal_->setCanceled(); - } - - starting(); // reset trajectory - - gh.setAccepted(); - - // Sends the trajectory along to the controller - active_goal_ = boost::shared_ptr(new GoalHandle(gh)); - commandTrajectory(share_member(gh.getGoal(),gh.getGoal()->trajectory), active_goal_); -} - -void BaseTrajectoryActionController::cancelCB(GoalHandle gh) -{ - if (active_goal_ && *active_goal_ == gh) - { - trajectory_msgs::JointTrajectory::Ptr empty(new trajectory_msgs::JointTrajectory); - empty->joint_names.resize(joints_.size()); - for (size_t j = 0; j < joints_.size(); ++j) - empty->joint_names[j] = joints_[j]->name; - commandTrajectory(empty); - - // Marks the current goal as canceled. - active_goal_->setCanceled(); - active_goal_ = boost::shared_ptr((GoalHandle*)NULL); - } -} diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/src/pr2_base_trajectory_action_controller.cpp b/jsk_pr2_robot/pr2_base_trajectory_action/src/pr2_base_trajectory_action_controller.cpp new file mode 100644 index 0000000000..2f2f01014f --- /dev/null +++ b/jsk_pr2_robot/pr2_base_trajectory_action/src/pr2_base_trajectory_action_controller.cpp @@ -0,0 +1,432 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * pr2_base_trajectory_action_controller.cpp + * Author: Yuki Furuta + */ + +#include +#include +#include +#include + +namespace pr2_base_trajectory_action { + + Controller::Controller() + : nh_(""), pnh_("~") + { + pnh_.param("update_frequency", frequency_, 40.0); + pnh_.param("stopped_velocity_tolerance", stopped_velocity_tolerance_, 0.01); + pnh_.param("goal_threshold", goal_threshold_, 0.01); + pnh_.param("goal_time_constraint", goal_time_constraint_, 0.0); + + pnh_.param("linear_x_joint_name", joints_.x.name, "base_link_x"); + pnh_.param("linear_y_joint_name", joints_.y.name, "base_link_y"); + pnh_.param("rotational_z_joint_name", joints_.yaw.name, "base_link_pan"); + pnh_.param(joints_.x.name + "/max_velocity", joints_.x.max_velocity, 0.2); + pnh_.param(joints_.y.name + "/max_velocity", joints_.y.max_velocity, 0.2); + pnh_.param(joints_.yaw.name + "/max_velocity", joints_.yaw.max_velocity, 0.2); + + cmd_vel_pub_ = nh_.advertise("command", 10); + odom_sub_ = nh_.subscribe("odom", 1, + boost::bind(&Controller::odomCallback, this, _1)); + + update_timer_ = nh_.createTimer(ros::Duration(1.0 / frequency_), + boost::bind(&Controller::update, this, _1)); + + as_.reset(new Server(nh_, + "follow_joint_trajectory", + boost::bind(&Controller::goalCallback, this, _1), + boost::bind(&Controller::cancelCallback, this, _1), + /* auto_start = */ false)); + as_->start(); + } + + Controller::~Controller() + {} + + void Controller::goalCallback(GoalHandle gh) + { + boost::mutex::scoped_lock lock(mutex_); + + ROS_DEBUG("new goal arrived"); + + // check joint names + std::set names; + for (int i = 0; i < 3; ++i) + names.insert(joints_[i].name); + std::set goal_joint_set(gh.getGoal()->trajectory.joint_names.begin(), + gh.getGoal()->trajectory.joint_names.end()); + if (names != goal_joint_set) { + ROS_ERROR("Joints on incoming goal don't match our joints"); + gh.setRejected(); + return; + } + + // send cancel to current active goal + if (active_goal_) + { + ROS_DEBUG("canceled active goal"); + active_goal_->setCanceled(); + } + + // validate points + if (!gh.getGoal()->trajectory.points.empty()) + { + int pos_size = gh.getGoal()->trajectory.points[0].positions.size(); + if (pos_size != gh.getGoal()->trajectory.points[0].velocities.size()) + { + ROS_ERROR("size of points and velocities must be the same"); + gh.setRejected(); + return; + } + else if (!gh.getGoal()->trajectory.points[0].accelerations.empty() && + pos_size != gh.getGoal()->trajectory.points[0].accelerations.size()) + { + ROS_ERROR("size of acclerations must be the same as points or 0"); + gh.setRejected(); + return; + } + } + + gh.setAccepted(); + active_goal_.reset(new GoalHandle(gh)); + + ROS_DEBUG("goal accepted"); + + if (gh.getGoal()->trajectory.points.empty()) + { // stop + clearTrajectory(); + return; + } + + // set new trajectory + setTrajectory(share_member(gh.getGoal(), gh.getGoal()->trajectory), active_goal_); + } + + void Controller::cancelCallback(GoalHandle gh) + { + boost::mutex::scoped_lock lock(mutex_); + if (active_goal_ && *active_goal_ == gh) + { + trajectory_msgs::JointTrajectory::Ptr empty(new trajectory_msgs::JointTrajectory); + boost::shared_ptr empty_gh = boost::shared_ptr((GoalHandle*)NULL); + empty->joint_names.resize(3); + for (int i = 0; i < 3; ++i) + empty->joint_names[i] = joints_[i].name; + setTrajectory(empty, empty_gh); + active_goal_->setCanceled(); + active_goal_ = boost::shared_ptr((GoalHandle*)NULL); + } + } + + void Controller::odomCallback(const nav_msgs::Odometry::ConstPtr & msg) + { + joints_.real_latest_time = msg->header.stamp; + joints_.x.position = msg->pose.pose.position.x; + joints_.y.position = msg->pose.pose.position.y; + joints_.yaw.position = 2.0 * atan2(msg->pose.pose.orientation.z, + msg->pose.pose.orientation.w); + + // for feedback msg + joints_.x.velocity = msg->twist.twist.linear.x; + joints_.y.velocity = msg->twist.twist.linear.y; + joints_.yaw.velocity = msg->twist.twist.angular.z; + } + + void Controller::update(const ros::TimerEvent& e) + { + if (!active_goal_) return; + if (active_traj_->size() <= 0) + { + ROS_ERROR("No segments in active trajectory"); + return; + } + + ros::Time now = e.current_real, last = e.last_real; + ros::Duration dt = now - last; + + BaseMotion next_motion; + active_traj_->motionAtTime(now.toSec(), next_motion); + // calc error + BaseMotion desired_motion, error_motion; + active_traj_->motionAtTime(joints_.real_latest_time.toSec(), desired_motion); + error_motion.x.position = desired_motion.x.position - joints_.x.position; + error_motion.y.position = desired_motion.y.position - joints_.y.position; + error_motion.yaw.position = angles::shortest_angular_distance(joints_.yaw.position, + desired_motion.yaw.position); + for(int i = 0; i < 3; ++i) + joints_[i].commanded_effort = error_motion[i].position * 1.0; // P gain: 1.0 + + // publish feedback + setFeedback(now, desired_motion, error_motion); + ROS_DEBUG_STREAM("desired: " << desired_motion); + ROS_DEBUG_STREAM("actual: x: " << joints_.x.position << + ", y: " << joints_.y.position << + ", th: " << joints_.yaw.position); + ROS_DEBUG_STREAM("error: " << error_motion); + + // check if goal reached + boost::shared_ptr gh; + bool has_goal = active_traj_->goalAtTime(now.toSec(), gh); + if (has_goal && gh == active_goal_) + { + ros::Time end_time(active_traj_->endTime()); + ROS_DEBUG_STREAM("time: " << now.toSec() << " -> end: " << end_time.toSec()); + + if(now > end_time) { + ROS_DEBUG_STREAM("current time exceeded goal end time"); + bool inside_goal_constraints = true; + for (size_t i = 0; i < 3 && inside_goal_constraints; ++i) + { + if (fabs(next_motion[i].velocity) < 1e-6) + { + if (fabs(joints_[i].velocity) > stopped_velocity_tolerance_) + { + ROS_WARN_STREAM("velocity of joint " << joints_[i].name << ": " << joints_[i].velocity << " > stopped_velocity_tolerance: " << stopped_velocity_tolerance_); + inside_goal_constraints = false; + } else if (fabs(error_motion[i].position) > goal_threshold_) { + ROS_WARN_STREAM("error of joint " << joints_[i].name << ": " << error_motion[i].position << " > goal_threshold: " << goal_threshold_); + inside_goal_constraints = false; + } + } + } + if (inside_goal_constraints) + { + active_goal_->setSucceeded(); + active_goal_ = boost::shared_ptr((GoalHandle*)NULL); + ROS_DEBUG_STREAM("set succeeded"); + } else if (now >= end_time + ros::Duration(goal_time_constraint_)) { + active_goal_->setAborted(); + active_goal_ = boost::shared_ptr((GoalHandle*)NULL); + ROS_DEBUG_STREAM("set aborted"); + } + } + + // check if new commands violates the max velocity + for (size_t i = 0; i < 3; ++i) + { + joints_[i].commanded_velocity = desired_motion[i].velocity + (use_pid ? joints_[i].commanded_effort : 0); + if (joints_[i].max_velocity < fabs(joints_[i].commanded_velocity)) + { + joints_[i].commanded_velocity *= joints_[i].max_velocity / fabs(joints_[i].commanded_velocity); + ROS_WARN_STREAM("joint " << joints_[i].name << " violates its velocity limit"); + } + } + + + // publish command velocity + cmd_vel_msg_.reset(new geometry_msgs::Twist); + double vx = joints_.x.commanded_velocity; + double vy = joints_.y.commanded_velocity; + double th = joints_.yaw.position; + ROS_DEBUG_STREAM("vx: " << vx << ", vy: " << vy << ", th: " << th); + cmd_vel_msg_->linear.x = vx * cos(th) + vy * sin(th); + cmd_vel_msg_->linear.y = vy * cos(th) - vx * sin(th); + cmd_vel_msg_->angular.z = joints_.yaw.commanded_velocity; + ROS_DEBUG_STREAM("cmd_vel x: " << cmd_vel_msg_->linear.x << + " ,y: " << cmd_vel_msg_->linear.y << + " ,th: " << cmd_vel_msg_->angular.z); + cmd_vel_pub_.publish(*cmd_vel_msg_); + } + } // function update + + void Controller::setFeedback(const ros::Time &stamp, const BaseMotion &desired, const BaseMotion &error) + { + if (!active_goal_) return; + control_msgs::FollowJointTrajectoryFeedback msg; + msg.header.stamp = stamp; + for (int i = 0; i < 3; ++i) { + msg.joint_names.push_back(joints_[i].name); + msg.desired.positions.push_back(desired[i].position); + msg.desired.velocities.push_back(desired[i].velocity); + msg.actual.positions.push_back(joints_[i].position); + msg.actual.velocities.push_back(joints_[i].velocity); + msg.error.positions.push_back(error[i].position); + msg.error.velocities.push_back(error[i].velocity); + } + active_goal_->publishFeedback(msg); + } + + void Controller::setTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &msg, + boost::shared_ptr &gh) + { + ROS_DEBUG("set trajectory"); + if (!active_traj_) clearTrajectory(); + ros::Time now = ros::Time::now(); + double msg_start_time = 0.0; + if (msg->header.stamp == ros::Time(0.0)) + msg_start_time = now.toSec(); + else msg_start_time = msg->header.stamp.toSec(); + Trajectory::ConstPtr prev_traj = active_traj_; + Trajectory::Ptr new_traj(new Trajectory(1)); + new_traj->segments[0].start_time = ros::Time::now().toSec() - 0.001; + new_traj->segments[0].duration = 0.0; + for (int i = 0; i < 3; ++i) + new_traj->segments[0].splines[i].coefficients[0] = joints_[i].position; + ROS_DEBUG("set prev trajectory"); + + // first, append previous trajectory to new traecjtory + int from_seg = -1, to_seg = -1; + while (from_seg + 1 < prev_traj->size() && + prev_traj->segments[from_seg + 1].start_time <= now.toSec()) + ++from_seg; + while (to_seg + 1 < prev_traj->size() && + prev_traj->segments[to_seg + 1].start_time < msg_start_time) + ++to_seg; + if (to_seg < from_seg) from_seg = to_seg; + + ROS_DEBUG_STREAM("use prev traj from " << from_seg << ", to " << to_seg); + + for (int i = std::max(from_seg, 0); i <= to_seg; ++i) { + new_traj->segments.push_back(prev_traj->segments[i]); + } + if (new_traj->size() == 0) + new_traj->segments.push_back(prev_traj->segments[prev_traj->size() - 1]); + + // look up table + int joint_idx_x = findIndex(msg->joint_names, joints_.x.name); + int joint_idx_y = findIndex(msg->joint_names, joints_.y.name); + int joint_idx_yaw = findIndex(msg->joint_names, joints_.yaw.name); + + ROS_DEBUG_STREAM("look up x: " << joint_idx_x << ", y: " << joint_idx_y << ", yaw: " << joint_idx_yaw); + + // calc rotational align from previous trajectory + Trajectory::Segment last_seg = new_traj->segments[new_traj->size() - 1]; + BaseMotion prev_motion; + last_seg.sample(msg_start_time, prev_motion); + double dist = angles::shortest_angular_distance(prev_motion.yaw.position, + msg->points[0].positions[joint_idx_yaw]); + double yaw_align = prev_motion.yaw.position + dist - msg->points[0].positions[joint_idx_yaw]; + + ROS_DEBUG_STREAM("yaw_align: " << yaw_align); + + // calc spline from trajectory + for (size_t i = 0; i < msg->points.size(); ++i) + { + Trajectory::Segment s; + s.duration = msg->points[i].time_from_start.toSec(); + if (i != 0) s.duration -= msg->points[i-1].time_from_start.toSec(); + s.start_time = msg_start_time + msg->points[i].time_from_start.toSec() - s.duration; + s.gh = gh; + + ROS_DEBUG_STREAM("s.start_time: " << s.start_time << ", s.duration: " << s.duration); + + if (prev_motion.hasAcceleration() && msg->points[i].accelerations.size() > 0) { + Motion mx(msg->points[i].positions[joint_idx_x], + msg->points[i].velocities[joint_idx_x], + msg->points[i].accelerations[joint_idx_x]); + QuinticSpline spx(prev_motion.x, mx, s.duration); + s.splines[0] = spx; + + Motion my(msg->points[i].positions[joint_idx_y], + msg->points[i].velocities[joint_idx_y], + msg->points[i].accelerations[joint_idx_y]); + QuinticSpline spy(prev_motion.y, my, s.duration); + s.splines[1] = spy; + + Motion myaw(msg->points[i].positions[joint_idx_yaw] + yaw_align, + msg->points[i].velocities[joint_idx_yaw], + msg->points[i].accelerations[joint_idx_yaw]); + QuinticSpline spyaw(prev_motion.yaw, myaw, s.duration); + s.splines[2] = spyaw; + new_traj->segments.push_back(s); + prev_motion.x = mx; + prev_motion.y = my; + prev_motion.yaw = myaw; + } else if (prev_motion.hasVelocity() && msg->points[i].velocities.size() > 0) { + ROS_DEBUG_THROTTLE(1.0, "velocity only"); + Motion mx(msg->points[i].positions[joint_idx_x], + msg->points[i].velocities[joint_idx_x]); + CubicSpline spx(prev_motion.x, mx, s.duration); + s.splines[0] = spx; + + Motion my(msg->points[i].positions[joint_idx_y], + msg->points[i].velocities[joint_idx_y]); + CubicSpline spy(prev_motion.y, my, s.duration); + s.splines[1] = spy; + + Motion myaw(msg->points[i].positions[joint_idx_yaw] + yaw_align, + msg->points[i].velocities[joint_idx_yaw]); + CubicSpline spyaw(prev_motion.yaw, myaw, s.duration); + s.splines[2] = spyaw; + new_traj->segments.push_back(s); + prev_motion.x = mx; + prev_motion.y = my; + prev_motion.yaw = myaw; + } else { + ROS_DEBUG_THROTTLE(1.0, "position only"); + Motion mx(msg->points[i].positions[joint_idx_x]); + Line spx(prev_motion.x, mx, s.duration); + s.splines[0] = spx; + + Motion my(msg->points[i].positions[joint_idx_y]); + Line spy(prev_motion.y, my, s.duration); + s.splines[1] = spy; + + Motion myaw(msg->points[i].positions[joint_idx_yaw] + yaw_align); + Line spyaw(prev_motion.yaw, myaw, s.duration); + s.splines[2] = spyaw; + new_traj->segments.push_back(s); + prev_motion.x = mx; + prev_motion.y = my; + prev_motion.yaw = myaw; + } + + // debugging + ROS_DEBUG_STREAM("start time: " << s.start_time << ", duration: " << s.duration); + for (int j = 0; j < 3; ++j) { + ROS_DEBUG_STREAM(" spline " << j << ":"); + for (int k = 0; k < 6; ++k) + ROS_DEBUG_STREAM(" " << k << ": " << s.splines[j].coefficients[k]); + } + } + + // commit new active trajectory + active_traj_ = new_traj; + ROS_DEBUG_STREAM("new active trajectory is set"); + } + + void Controller::clearTrajectory() + { + active_traj_.reset(new Trajectory(1)); + active_traj_->segments[0].start_time = ros::Time::now().toSec() - 0.001; + active_traj_->segments[0].duration = 0.0; + for (int i = 0; i < 3; ++i) + active_traj_->segments[0].splines[i].coefficients[0] = joints_[i].position; + ROS_DEBUG("clear trajectory"); + } +} // namespace diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/src/pr2_base_trajectory_action_controller_node.cpp b/jsk_pr2_robot/pr2_base_trajectory_action/src/pr2_base_trajectory_action_controller_node.cpp new file mode 100644 index 0000000000..f8a8786918 --- /dev/null +++ b/jsk_pr2_robot/pr2_base_trajectory_action/src/pr2_base_trajectory_action_controller_node.cpp @@ -0,0 +1,53 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * pr2_base_trajectory_action_controller_node.cpp + * Author: Yuki Furuta + */ + +#include +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "pr2_base_trajectory_action_controller"); + + pr2_base_trajectory_action::Controller c; + ros::spin(); + + return 0; +} + diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/test/pr2_base_trajectory_action.test b/jsk_pr2_robot/pr2_base_trajectory_action/test/pr2_base_trajectory_action.test new file mode 100644 index 0000000000..ebc6d1bf00 --- /dev/null +++ b/jsk_pr2_robot/pr2_base_trajectory_action/test/pr2_base_trajectory_action.test @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/test/spline_test.cpp b/jsk_pr2_robot/pr2_base_trajectory_action/test/spline_test.cpp new file mode 100644 index 0000000000..2e9b60797e --- /dev/null +++ b/jsk_pr2_robot/pr2_base_trajectory_action/test/spline_test.cpp @@ -0,0 +1,97 @@ +/* + * spline_test.cpp + * Author: Yuki Furuta + */ + +#include +#include +#include + +using namespace std; +using namespace pr2_base_trajectory_action; + +TEST(TestSuite, testSpline) +{ + Motion mx0(0.0, 0.23599999999283272); + Motion mx1(1.0, 0.0); + Motion my0(0.0, 0.0); + Motion my1(0.0, 0.0); + Motion myaw0(0.0, 0.0); + Motion myaw1(0.0, 0.0); + double duration = 4.237288128; + CubicSpline spx(mx0, mx1, duration); + CubicSpline spy(my0, my1, duration); + CubicSpline spyaw(myaw0, myaw1, duration); + + Trajectory::Segment s; + s.start_time = 0.0; + s.duration = duration; + s.splines[0] = spx; + s.splines[1] = spy; + s.splines[2] = spyaw; + + cout << "x: " << endl; + for (int i = 0; i < 6; ++i) + cout << " " << i << ": " << spx.coefficients[i] << endl; + std::vector spx_true(6, 0.0); + spx_true[1] = 0.236; + spx_true[2] = 0.055696; + spx_true[3] = -0.0131443; + for(int i = 0; i < 6; ++i) + ASSERT_NEAR(spx.coefficients[i], spx_true[i], 0.001); + + cout << "y: " << endl; + for (int i = 0; i < 6; ++i) + cout << " " << i << ": " << spy.coefficients[i] << endl; + for(int i = 0; i < 6; ++i) + ASSERT_NEAR(spy.coefficients[i], 0.0, 0.001); + + cout << "yaw: " << endl; + for (int i = 0; i < 6; ++i) + cout << " " << i << ": " << spyaw.coefficients[i] << endl; + for(int i = 0; i < 6; ++i) + ASSERT_NEAR(spyaw.coefficients[i], 0.0, 0.001); + + std::vector sample_true; + sample_true.push_back(0.0); + sample_true.push_back(0.0754577); + sample_true.push_back(0.158811); + sample_true.push_back(0.247932); + sample_true.push_back(0.340689); + sample_true.push_back(0.434954); + sample_true.push_back(0.528598); + sample_true.push_back(0.61949); + sample_true.push_back(0.705503); + sample_true.push_back(0.784505); + sample_true.push_back(0.854369); + sample_true.push_back(0.912964); + sample_true.push_back(0.958162); + sample_true.push_back(0.987832); + sample_true.push_back(0.999846); + + int i = 0; + for (double t = 0.0; t < duration; t += 0.3) { + Motion x,y,yaw; + spx.sampleWithTimeBounds(duration, t, x); + spy.sampleWithTimeBounds(duration, t, y); + spyaw.sampleWithTimeBounds(duration, t, yaw); + ASSERT_NEAR(x.position, sample_true[i], 0.001); + cout << "t: " << t << ", x: " << x.position << " y: " << y.position << " yaw: " << yaw.position << endl; + ++i; + } + + i = 0; + for (double t = 0.0; t < duration; t += 0.3) { + BaseMotion b; + s.sample(t, b); + cout << "t: " << t << ", " << b << endl; + ASSERT_NEAR(b.x.position, sample_true[i], 0.001); + ++i; + } +} + + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/test/test_pr2_base_trajectory_action.l b/jsk_pr2_robot/pr2_base_trajectory_action/test/test_pr2_base_trajectory_action.l new file mode 100644 index 0000000000..d8b9c32e60 --- /dev/null +++ b/jsk_pr2_robot/pr2_base_trajectory_action/test/test_pr2_base_trajectory_action.l @@ -0,0 +1,69 @@ +;; test_pr2_base_trajectory_action.l +;; Author: Yuki Furuta + +(require :unittest "lib/llib/unittest.l") + +(init-unit-test) + +;; pr2 on gazebo does not keep velocity commanded from controller. +;; so we instead had to write test case with wide threshold +#| +(deftest test-go-pos-unsafe () + (pr2-init) + (setq *start-cds* (send *ri* :state :worldcoords)) + + (send *ri* :go-pos-unsafe 1.0 0) + (setq *go-cds* (send *ri* :state :worldcoords)) + (warn "cds: ~A~%" *go-cds*) + (assert (eps-v= (send *go-cds* :worldpos) (float-vector 1000 0 0) 50) + "go-pos 1 0 0") + (setq *prev-pos* (send *start-cds* :copy-worldcoords)) + (send *ri* :go-pos-unsafe 0 1.0) + (setq *go-cds* (send *ri* :state :worldcoords)) + (warn "cds: ~A~%" *go-cds*) + (assert (eps-v= (send *go-cds* :worldpos) (float-vector 1000 1000 0) 50) + "go-pos 0 1 0") + + (send *ri* :go-pos-unsafe 0 0 90) + (setq *go-cds* (send *ri* :state :worldcoords)) + (warn "cds: ~A~%" *go-cds*) + (assert (< (abs (- (caar (send *go-cds* :rpy-angle)) pi/2)) (deg2rad 10)) + "go-pos 0 0 pi/2")) +|# + +(deftest test-go-pos-unsafe () + (load "package://pr2eus/pr2-interface.l") + (pr2-init) + ;; wait for action controller + (setq *base-traj-action* (pr2-interface-move-base-trajectory-action *ri*)) + (setq *cnt* 0) + (while (not (send *base-traj-action* :wait-for-server 1)) + (ros::ros-warn "waiting for server ~A" (send *base-traj-action* :name)) + (unix:sleep 3) + (when (> (inc *cnt*) 100) + (assert nil "server not started"))) + + (setq *start-cds* (send *ri* :state :worldcoords)) + + (send *ri* :go-pos-unsafe 1.0 0) + (setq *go-cds* (send *ri* :state :worldcoords)) + (warn "cds: ~A~%" *go-cds*) + (assert (> (norm (send *start-cds* :difference-position *go-cds*)) 50) + "go-pos 1 0 0") + (setq *prev-cds* (send *start-cds* :copy-worldcoords)) + (send *ri* :go-pos-unsafe 0 1.0) + (setq *go-cds* (send *ri* :state :worldcoords)) + (warn "cds: ~A~%" *go-cds*) + (assert (> (norm (send *prev-cds* :difference-position *go-cds*)) 50) + "go-pos 0 1 0") + (setq *prev-cds* (send *go-cds* :copy-worldcoords)) + + (send *ri* :go-pos-unsafe 0 0 90) + (setq *go-cds* (send *ri* :state :worldcoords)) + (warn "cds: ~A~%" *go-cds*) + (assert (> (norm (send *go-cds* :difference-rotation *prev-cds*)) (deg2rad 10)) + "go-pos 0 0 pi/2") +) + +(run-all-tests) +(exit)