Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22570] pkg/trunk
|
From: <stu...@us...> - 2009-08-21 19:05:53
|
Revision: 22570
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22570&view=rev
Author: stuglaser
Date: 2009-08-21 19:05:43 +0000 (Fri, 21 Aug 2009)
Log Message:
-----------
Moved the JointPDController and CartesianTrajectoryController out of robot_mechanism_controllers
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml
pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp
pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt
pkg/trunk/sandbox/experimental_controllers/controller_plugins.xml
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_trajectory_controller.h
Added Paths:
-----------
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_pd_controller.h
pkg/trunk/sandbox/experimental_controllers/src/cartesian_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_pd_controller.cpp
pkg/trunk/sandbox/experimental_controllers/srv/GetPDCommand.srv
pkg/trunk/sandbox/experimental_controllers/srv/SetPDCommand.srv
Removed Paths:
-------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_trajectory_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_pd_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_pd_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/srv/GetPDCommand.srv
pkg/trunk/controllers/robot_mechanism_controllers/srv/SetPDCommand.srv
Modified: pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-08-21 19:02:00 UTC (rev 22569)
+++ pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-08-21 19:05:43 UTC (rev 22570)
@@ -7,14 +7,12 @@
gensrv()
set(_srcs
src/controller_manifest.cpp
- src/cartesian_trajectory_controller.cpp
src/cartesian_pose_controller.cpp
src/cartesian_twist_controller.cpp
src/cartesian_wrench_controller.cpp
src/joint_effort_controller.cpp
src/joint_position_controller.cpp
src/joint_velocity_controller.cpp
- src/joint_pd_controller.cpp
src/joint_ud_calibration_controller.cpp
src/trigger_controller.cpp
)
Modified: pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml 2009-08-21 19:02:00 UTC (rev 22569)
+++ pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml 2009-08-21 19:05:43 UTC (rev 22570)
@@ -10,11 +10,7 @@
<class name="JointUDCalibrationController" type="JointUDCalibrationController" base_class_type="Controller" />
- <class name="CartesianTrajectoryController" type="CartesianTrajectoryController" base_class_type="Controller" />
-
<class name="TriggerController" type="TriggerController" base_class_type="Controller" />
<class name="TriggerControllerNode" type="TriggerControllerNode" base_class_type="Controller" />
- <class name="JointPDController" type="JointPDController" base_class_type="Controller" />
- <class name="JointPDControllerNode" type="JointPDControllerNode" base_class_type="Controller" />
</library>
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_trajectory_controller.h 2009-08-21 19:02:00 UTC (rev 22569)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_trajectory_controller.h 2009-08-21 19:05:43 UTC (rev 22570)
@@ -1,113 +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: Wim Meeussen
- */
-
-#ifndef CARTESIAN_TRAJECTORY_CONTROLLER_H
-#define CARTESIAN_TRAJECTORY_CONTROLLER_H
-
-#include <vector>
-#include <string>
-#include <kdl/chain.hpp>
-#include <kdl/frames.hpp>
-#include <kdl/velocityprofile_trap.hpp>
-#include <tf/transform_listener.h>
-#include <tf/message_notifier.h>
-#include <ros/node.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <controller_interface/controller.h>
-#include <robot_mechanism_controllers/cartesian_pose_controller.h>
-#include <deprecated_srvs/MoveToPose.h>
-#include <std_srvs/Empty.h>
-#include <boost/scoped_ptr.hpp>
-
-namespace controller {
-
-
-class CartesianTrajectoryController : public Controller
-{
-public:
- CartesianTrajectoryController();
- ~CartesianTrajectoryController();
-
- bool init(mechanism::RobotState *robot_state, const ros::NodeHandle& n);
-
- bool starting();
- void update();
- bool moveTo(const geometry_msgs::PoseStamped& pose,
- const geometry_msgs::Twist& tolerance=geometry_msgs::Twist(), double duration=0);
-
-
-private:
- KDL::Frame getPose();
- void TransformToFrame(const tf::Transform& trans, KDL::Frame& frame);
-
- // topic
- void command(const tf::MessageNotifier<geometry_msgs::PoseStamped>::MessagePtr& pose_msg);
-
- // service calls
- bool moveTo(deprecated_srvs::MoveToPose::Request &req, deprecated_srvs::MoveToPose::Response &resp);
- bool preempt(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
-
- ros::NodeHandle node_;
- ros::ServiceServer move_to_srv_, preempt_srv_;
-
- std::string controller_name_;
- double last_time_, time_started_, time_passed_, max_duration_;
- bool is_moving_, request_preempt_, exceed_tolerance_;
-
- KDL::Frame pose_begin_, pose_end_, pose_current_;
- KDL::Twist twist_current_, tolerance_;
-
- // robot structure
- mechanism::RobotState *robot_state_;
- mechanism::Chain chain_;
-
- // kdl stuff for kinematics
- KDL::Chain kdl_chain_;
- boost::scoped_ptr<KDL::ChainFkSolverPos> jnt_to_pose_solver_;
- KDL::JntArray jnt_pos_;
-
- // motion profiles
- std::vector<KDL::VelocityProfile_Trap> motion_profile_;
-
- // pose controller
- CartesianPoseController* pose_controller_;
-
- tf::TransformListener tf_;
- boost::scoped_ptr<tf::MessageNotifier<geometry_msgs::PoseStamped> > command_notifier_;
-
- std::string root_name_;
-};
-
-
-}
-#endif
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_pd_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_pd_controller.h 2009-08-21 19:02:00 UTC (rev 22569)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_pd_controller.h 2009-08-21 19:05:43 UTC (rev 22570)
@@ -1,168 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * 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 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.
- *********************************************************************/
-
-#pragma once
-
-/***************************************************/
-/*! \class controller::JointPDController
- \brief Joint Velocity Controller
-
- This class closes the loop around velocity using
- a pid loop.
-
- Example config:<br>
-
- <controller type="JointPDController" name="controller_name"><br>
- <joint name="joint_to_control"><br>
- <pid p="1.0" i="2.0" d="3.0" iClamp="4.0" /><br>
- </joint>
- </controller>
-*/
-/***************************************************/
-
-#include <ros/node.h>
-
-#include <controller_interface/controller.h>
-#include <control_toolbox/pid.h>
-#include "misc_utils/advertised_service_guard.h"
-#include "misc_utils/subscription_guard.h"
-
-// Services
-#include <robot_mechanism_controllers/SetPDCommand.h>
-#include <robot_mechanism_controllers/GetPDCommand.h>
-#include <deprecated_msgs/JointCmd.h>
-
-namespace controller
-{
-
- class JointPDController : public Controller
- {
- public:
-
- JointPDController();
- ~JointPDController();
-
- /*!
- * \brief Functional way to initialize limits and gains.
- * \param pid Pid gain values.
- * \param joint_name Name of joint we want to control.
- * \param *robot The robot.
- */
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- bool init(mechanism::RobotState *robot, const std::string &joint_name,const control_toolbox::Pid &pid);
-
- /*!
- * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
- *
- * \param double pos Velocity command to issue
- */
- void setCommand(double command, double command_dot);
-
- /*!
- * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
- */
- void getCommand(deprecated_msgs::JointCmd & cmd);
-
-
- /*!
- * \brief Issues commands to the joint. Should be called at regular intervals
- */
- virtual void update();
-
- void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
- void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min);
-
- std::string getJointName();
- mechanism::JointState *joint_state_; /**< Joint we're controlling. */
-
- /*!
- * \brief Reset the internal PID controllers
- */
- void reset();
-
- private:
-
- mechanism::RobotState *robot_; /**< Pointer to robot structure. */
- control_toolbox::Pid pid_controller_; /**< Internal PID controller. */
- double last_time_; /**< Last time stamp of update. */
- double command_; /**< Last commanded position. */
- double command_dot_;
- double command_t_; /**< Last commanded position. */
- double command_dot_t_;
-
- /*!
- * \brief mutex lock for setting and getting commands
- */
- pthread_mutex_t joint_pd_controller_lock_;
-
- };
-
-/***************************************************/
-/*! \class controller::JointPDControllerNode
- \brief Joint PD Controller ROS Node
-
- This class closes the loop around velocity using
- a pid loop.
-
-*/
-/***************************************************/
-
- class JointPDControllerNode : public Controller
- {
- public:
-
- JointPDControllerNode();
- ~JointPDControllerNode();
-
- void update();
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- // Topics
- void setCommand();
-
- private:
-
- //node stuff
- std::string service_prefix_; /**< The name of the controller. */
- ros::Node *node_;
- SubscriptionGuard guard_set_command_; /**< Makes sure the subscription goes down neatly. */
-
- //msgs
- deprecated_msgs::JointCmd cmd_; /**< The command from the subscription. */
-
- //controller
- JointPDController *c_; /**< The controller. */
-
- };
-}
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp 2009-08-21 19:02:00 UTC (rev 22569)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp 2009-08-21 19:05:43 UTC (rev 22570)
@@ -1,348 +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: Wim Meeussen
- */
-
-
-
-#include <algorithm>
-#include <mechanism_control/mechanism_control.h>
-#include "kdl/chainfksolverpos_recursive.hpp"
-#include "robot_mechanism_controllers/cartesian_trajectory_controller.h"
-
-
-using namespace KDL;
-using namespace tf;
-using namespace ros;
-
-
-namespace controller {
-
-ROS_REGISTER_CONTROLLER(CartesianTrajectoryController)
-
-
-CartesianTrajectoryController::CartesianTrajectoryController()
-: jnt_to_pose_solver_(NULL),
- motion_profile_(6, VelocityProfile_Trap(0,0))
-{}
-
-CartesianTrajectoryController::~CartesianTrajectoryController()
-{}
-
-
-
-bool CartesianTrajectoryController::init(mechanism::RobotState *robot_state, const NodeHandle& n)
-{
- node_ = n;
-
- // get name of root and tip from the parameter server
- std::string tip_name;
- if (!node_.getParam("root_name", root_name_)){
- ROS_ERROR("CartesianTrajectoryController: No root name found on parameter server");
- return false;
- }
- if (!node_.getParam("tip_name", tip_name)){
- ROS_ERROR("CartesianTrajectoryController: No tip name found on parameter server");
- return false;
- }
-
- // test if we got robot pointer
- assert(robot_state);
- robot_state_ = robot_state;
-
- // create robot chain from root to tip
- if (!chain_.init(robot_state->model_, root_name_, tip_name))
- return false;
- chain_.toKDL(kdl_chain_);
-
- // create solver
- jnt_to_pose_solver_.reset(new ChainFkSolverPos_recursive(kdl_chain_));
- jnt_pos_.resize(kdl_chain_.getNrOfJoints());
-
- // initialize motion profile
- double max_vel_trans, max_vel_rot, max_acc_trans, max_acc_rot;
- node_.param("max_vel_trans", max_vel_trans, 0.0) ;
- node_.param("max_vel_rot", max_vel_rot, 0.0) ;
- node_.param("max_acc_trans", max_acc_trans, 0.0) ;
- node_.param("max_acc_rot", max_acc_rot, 0.0) ;
- for (unsigned int i=0; i<3; i++){
- motion_profile_[i ].SetMax(max_vel_trans, max_acc_trans);
- motion_profile_[i+3].SetMax(max_vel_rot, max_acc_rot);
- }
-
- // get a pointer to the pose controller
- std::string output;
- if (!node_.getParam("output", output)){
- ROS_ERROR("CartesianTrajectoryController: No ouptut name found on parameter server");
- return false;
- }
- if (!getController<CartesianPoseController>(output, AFTER_ME, pose_controller_)){
- ROS_ERROR("CartesianTrajectoryController: could not connect to pose controller");
- return false;
- }
-
- // subscribe to pose commands
- command_notifier_.reset(new MessageNotifier<geometry_msgs::PoseStamped>(tf_,
- boost::bind(&CartesianTrajectoryController::command, this, _1),
- node_.getNamespace() + "/command", root_name_, 1));
- // advertise services
- move_to_srv_ = node_.advertiseService("move_to", &CartesianTrajectoryController::moveTo, this);
- preempt_srv_ = node_.advertiseService("preempt", &CartesianTrajectoryController::preempt, this);
-
- return true;
-}
-
-
-
-
-bool CartesianTrajectoryController::moveTo(const geometry_msgs::PoseStamped& pose, const geometry_msgs::Twist& tolerance, double duration)
-{
- // don't do anything when still moving
- if (is_moving_) return false;
-
- // convert message to transform
- Stamped<Pose> pose_stamped;
- poseStampedMsgToTF(pose, pose_stamped);
-
- // convert to reference frame of root link of the controller chain
- Duration timeout = Duration().fromSec(2.0);
- std::string error_msg;
- if (!tf_.waitForTransform(root_name_, pose.header.frame_id, pose_stamped.stamp_, timeout, Duration(0.01), &error_msg)){
- ROS_ERROR("CartesianTrajectoryController: %s", error_msg.c_str());
- return false;
- }
- tf_.transformPose(root_name_, pose_stamped, pose_stamped);
-
- // trajectory from pose_begin to pose_end
- TransformToFrame(pose_stamped, pose_end_);
- pose_begin_ = pose_current_;
-
- max_duration_ = 0;
- Twist twist_move = diff(pose_begin_, pose_end_);
-
- // Set motion profiles
- for (unsigned int i=0; i<6; i++){
- motion_profile_[i].SetProfileDuration( 0, twist_move(i), duration);
- max_duration_ = max( max_duration_, motion_profile_[i].Duration() );
- }
-
- // Rescale trajectories to maximal duration
- for (unsigned int i=0; i<6; i++)
- motion_profile_[i].SetProfileDuration( 0, twist_move(i), max_duration_ );
-
- // set tolerance
- tolerance_.vel(0) = tolerance.linear.x;
- tolerance_.vel(1) = tolerance.linear.y;
- tolerance_.vel(2) = tolerance.linear.z;
- tolerance_.rot(0) = tolerance.angular.x;
- tolerance_.rot(1) = tolerance.angular.y;
- tolerance_.rot(2) = tolerance.angular.z;
-
- time_passed_ = 0;
-
- exceed_tolerance_ = false;
- request_preempt_ = false;
- is_moving_ = true;
-
- ROS_INFO("CartesianTrajectoryController: %s will move to new pose in %f seconds", controller_name_.c_str(), max_duration_);
-
- return true;
-}
-
-
-
-bool CartesianTrajectoryController::starting()
-{
- // time
- last_time_ = robot_state_->hw_->current_time_;
-
- // set desired pose to current pose
- pose_current_ = getPose();
- twist_current_ = Twist::Zero();
-
- // start not moving
- is_moving_ = false;
- request_preempt_ = false;
-
- return true;
-}
-
-
-
-
-void CartesianTrajectoryController::update()
-{
- // get time
- double time = robot_state_->hw_->current_time_;
- double dt = time - last_time_;
- last_time_ = time;
-
- // preempt trajectory
- if (request_preempt_){
- twist_current_ = Twist::Zero();
- is_moving_ = false;
- }
-
- // if we are moving
- if (is_moving_){
- time_passed_ += dt;
-
- // check tolerance
- for (unsigned int i=0; i<6; i++){
- if (tolerance_[i] != 0 && pose_controller_->twist_error_[i] > tolerance_[i]){
- exceed_tolerance_ = true;
- is_moving_ = false;
- }
- }
-
- // ended trajectory
- if (time_passed_ > max_duration_){
- twist_current_ = Twist::Zero();
- pose_current_ = pose_end_;
- is_moving_ = false;
- }
- // still in trajectory
- else{
- // pose
- Twist twist_begin_current = Twist(Vector(motion_profile_[0].Pos(time_passed_),
- motion_profile_[1].Pos(time_passed_),
- motion_profile_[2].Pos(time_passed_)),
- Vector(motion_profile_[3].Pos(time_passed_),
- motion_profile_[4].Pos(time_passed_),
- motion_profile_[5].Pos(time_passed_)) );
- pose_current_ = Frame( pose_begin_.M * Rot( pose_begin_.M.Inverse( twist_begin_current.rot ) ),
- pose_begin_.p + twist_begin_current.vel);
-
- // twist
- for(unsigned int i=0; i<6; i++)
- twist_current_(i) = motion_profile_[i].Vel( time_passed_ );
- }
- }
-
- // send output to pose controller
- pose_controller_->pose_desi_ = pose_current_;
- pose_controller_->twist_ff_ = twist_current_;
-}
-
-
-
-Frame CartesianTrajectoryController::getPose()
-{
- // get the joint positions and velocities
- chain_.getPositions(robot_state_->joint_states_, jnt_pos_);
-
- // get cartesian pose
- Frame result;
- jnt_to_pose_solver_->JntToCart(jnt_pos_, result);
-
- return result;
-}
-
-
-bool CartesianTrajectoryController::moveTo(deprecated_srvs::MoveToPose::Request &req,
- deprecated_srvs::MoveToPose::Response &resp)
-{
- ROS_DEBUG("in cartesian traj move_to service");
-
- if (!moveTo(req.pose, req.tolerance, 0.0)){
- ROS_ERROR("CartesianTrajectoryController: not starting trajectory because either previous one is still running or the transform frame could not be found");
- return false;
- }
-
- ros::Duration timeout = Duration().fromSec(3.0);
- ros::Duration traj_duration = Duration().fromSec(max_duration_);
- ros::Time start_time = ros::Time::now();
-
- while (is_moving_){
- Duration().fromSec(0.1).sleep();
- if (ros::Time::now() > start_time + timeout + traj_duration){
- ROS_ERROR("CartesianTrajectoryController: timeout");
- return false;
- }
- }
-
-
- if (request_preempt_){
- ROS_ERROR("CartesianTrajectoryController: trajectory preempted");
- return false;
- }
- else if (exceed_tolerance_){
- ROS_ERROR("CartesianTrajectoryController: exceeded trajectory tolerance");
- return false;
- }
- else{
- ROS_DEBUG("CartesianTrajectoryController: moveto finished successfully");
- return true;
- }
-}
-
-
-
-void CartesianTrajectoryController::command(const MessageNotifier<geometry_msgs::PoseStamped>::MessagePtr& pose_msg)
-{
- moveTo(*pose_msg);
-}
-
-
-bool CartesianTrajectoryController::preempt(std_srvs::Empty::Request &req,
- std_srvs::Empty::Response &resp)
-{
- // you can only preempt is the robot is moving
- if (!is_moving_)
- return false;
-
- request_preempt_ = true;
-
- // wait for robot to stop moving
- Duration sleep_time = Duration().fromSec(0.01);
- while (is_moving_)
- sleep_time.sleep();
-
- return true;
-}
-
-
-
-void CartesianTrajectoryController::TransformToFrame(const Transform& trans, Frame& frame)
-{
- frame.p(0) = trans.getOrigin().x();
- frame.p(1) = trans.getOrigin().y();
- frame.p(2) = trans.getOrigin().z();
-
- double Rz, Ry, Rx;
- trans.getBasis().getEulerZYX(Rz, Ry, Rx);
- frame.M = Rotation::EulerZYX(Rz, Ry, Rx);
-}
-
-
-
-}; // namespace
-
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp 2009-08-21 19:02:00 UTC (rev 22569)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp 2009-08-21 19:05:43 UTC (rev 22570)
@@ -31,11 +31,9 @@
#include "controller_interface/controller.h"
#include "robot_mechanism_controllers/cartesian_pose_controller.h"
-#include "robot_mechanism_controllers/cartesian_trajectory_controller.h"
#include "robot_mechanism_controllers/cartesian_twist_controller.h"
#include "robot_mechanism_controllers/cartesian_wrench_controller.h"
#include "robot_mechanism_controllers/joint_effort_controller.h"
-#include "robot_mechanism_controllers/joint_pd_controller.h"
#include "robot_mechanism_controllers/joint_position_controller.h"
#include "robot_mechanism_controllers/joint_ud_calibration_controller.h"
#include "robot_mechanism_controllers/joint_velocity_controller.h"
@@ -54,11 +52,5 @@
PLUGINLIB_REGISTER_CLASS(JointUDCalibrationController, JointUDCalibrationController, Controller)
-PLUGINLIB_REGISTER_CLASS(CartesianTrajectoryController, CartesianTrajectoryController, Controller)
-
PLUGINLIB_REGISTER_CLASS(TriggerController, TriggerController, Controller)
PLUGINLIB_REGISTER_CLASS(TriggerControllerNode, TriggerControllerNode, Controller)
-
-PLUGINLIB_REGISTER_CLASS(JointPDController, JointPDController, Controller)
-PLUGINLIB_REGISTER_CLASS(JointPDControllerNode, JointPDControllerNode, Controller)
-
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_pd_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_pd_controller.cpp 2009-08-21 19:02:00 UTC (rev 22569)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_pd_controller.cpp 2009-08-21 19:05:43 UTC (rev 22570)
@@ -1,212 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * 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 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.
- *********************************************************************/
-
-#include <robot_mechanism_controllers/joint_pd_controller.h>
-#include <angles/angles.h>
-
-using namespace std;
-using namespace controller;
-
-ROS_REGISTER_CONTROLLER(JointPDController)
-
-JointPDController::JointPDController()
-: joint_state_(NULL), robot_(NULL), last_time_(0), command_(0), command_dot_(0)
-{
- pthread_mutex_init(&joint_pd_controller_lock_,NULL);
-}
-
-JointPDController::~JointPDController()
-{
-}
-
-bool JointPDController::init(mechanism::RobotState *robot, const std::string &joint_name,
- const control_toolbox::Pid &pid)
-
-{
- assert(robot);
- robot_ = robot;
- last_time_ = robot->hw_->current_time_;
-
- joint_state_ = robot_->getJointState(joint_name);
- if (!joint_state_)
- {
- fprintf(stderr, "JointPDController could not find joint named \"%s\"\n",
- joint_name.c_str());
- return false;
- }
-
- pid_controller_ = pid;
-
- return true;
-
- command_= 0;
- command_dot_ = 0;
-}
-
-bool JointPDController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- assert(robot);
-
- TiXmlElement *j = config->FirstChildElement("joint");
- if (!j)
- {
- fprintf(stderr, "JointPDController was not given a joint\n");
- return false;
- }
-
- const char *jn = j->Attribute("name");
- std::string joint_name = jn ? jn : "";
-
- TiXmlElement *p = j->FirstChildElement("pid");
- control_toolbox::Pid pid;
- if (p)
- pid.initXml(p);
- else
- fprintf(stderr, "JointPDController's config did not specify the default pid parameters.\n");
-
- return init(robot, joint_name, pid);
-}
-
-void JointPDController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min)
-{
- pid_controller_.setGains(p,i,d,i_max,i_min);
-}
-
-void JointPDController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
-{
- pid_controller_.getGains(p,i,d,i_max,i_min);
-}
-
-std::string JointPDController::getJointName()
-{
- return(joint_state_->joint_->name_);
-}
-
-// Set the joint velocity command
-void JointPDController::setCommand(double command, double command_dot)
-{
- pthread_mutex_lock(&joint_pd_controller_lock_);
- command_t_ = command;
- command_dot_t_ = command_dot;
- pthread_mutex_unlock(&joint_pd_controller_lock_);
-}
-
-// Return the current command
-void JointPDController::getCommand(deprecated_msgs::JointCmd & cmd)
-{
- pthread_mutex_lock(&joint_pd_controller_lock_);
- cmd.names[0]= joint_state_->joint_->name_;
- cmd.positions[0] = command_t_;
- cmd.velocity[0] = command_dot_t_;
- pthread_mutex_unlock(&joint_pd_controller_lock_);
-}
-
-void JointPDController::reset()
-{
- pid_controller_.reset();
-}
-
-void JointPDController::update()
-{
- double error(0), error_dot(0);
- double time = robot_->hw_->current_time_;
-
- if(pthread_mutex_trylock(&joint_pd_controller_lock_) == 0)
- {
- command_ = command_t_;
- command_dot_ = command_dot_t_;
- pthread_mutex_unlock(&joint_pd_controller_lock_);
- }
-
- error_dot = joint_state_->velocity_ - command_dot_;
-// error = joint_state_->position_ - command_;
-
- if(joint_state_->joint_->type_ == mechanism::JOINT_ROTARY)
- {
- angles::shortest_angular_distance_with_limits(command_, joint_state_->position_, joint_state_->joint_->joint_limit_min_, joint_state_->joint_->joint_limit_max_,error);
-
- }
- else if(joint_state_->joint_->type_ == mechanism::JOINT_CONTINUOUS)
- {
- error = angles::shortest_angular_distance(command_, joint_state_->position_);
- }
- else //prismatic
- {
- error = joint_state_->position_ - command_;
- }
-
-// error = joint_state_->position_ - command_;
-
- joint_state_->commanded_effort_ = pid_controller_.updatePid(error, error_dot, time - last_time_);
- last_time_ = time;
-}
-
-
-//------ Joint PD controller node --------
-ROS_REGISTER_CONTROLLER(JointPDControllerNode)
-
-JointPDControllerNode::JointPDControllerNode(): node_(ros::Node::instance())
-{
- c_ = new JointPDController();
-}
-
-JointPDControllerNode::~JointPDControllerNode()
-{
- delete c_;
-}
-
-void JointPDControllerNode::update()
-{
- c_->update();
-}
-
-bool JointPDControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- assert(node_);
- service_prefix_ = config->Attribute("name");
-
- if (!c_->initXml(robot, config))
- return false;
- //subscriptions
- node_->subscribe(service_prefix_ + "/set_command", cmd_, &JointPDControllerNode::setCommand, this, 1);
- guard_set_command_.set(service_prefix_ + "/set_command");
-
- return true;
-}
-
-void JointPDControllerNode::setCommand()
-{
- c_->setCommand(cmd_.positions[0],cmd_.velocity[0]);
-}
-
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/srv/GetPDCommand.srv
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/srv/GetPDCommand.srv 2009-08-21 19:02:00 UTC (rev 22569)
+++ pkg/trunk/controllers/robot_mechanism_controllers/srv/GetPDCommand.srv 2009-08-21 19:05:43 UTC (rev 22570)
@@ -1,4 +0,0 @@
----
-float64 time
-float64 command
-float64 command_dot
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/srv/SetPDCommand.srv
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/srv/SetPDCommand.srv 2009-08-21 19:02:00 UTC (rev 22569)
+++ pkg/trunk/controllers/robot_mechanism_controllers/srv/SetPDCommand.srv 2009-08-21 19:05:43 UTC (rev 22570)
@@ -1,5 +0,0 @@
-float64 command
-float64 command_dot
----
-float64 command
-float64 command_dot
Modified: pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt 2009-08-21 19:02:00 UTC (rev 22569)
+++ pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt 2009-08-21 19:05:43 UTC (rev 22570)
@@ -27,6 +27,8 @@
src/controller_manifest.cpp
src/trajectory_controller.cpp
src/pid_position_velocity_controller.cpp
+ src/cartesian_trajectory_controller.cpp
+ src/joint_pd_controller.cpp
src/cartesian_tff_controller.cpp
src/cartesian_twist_controller_ik.cpp
Modified: pkg/trunk/sandbox/experimental_controllers/controller_plugins.xml
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/controller_plugins.xml 2009-08-21 19:02:00 UTC (rev 22569)
+++ pkg/trunk/sandbox/experimental_controllers/controller_plugins.xml 2009-08-21 19:05:43 UTC (rev 22570)
@@ -25,6 +25,9 @@
<class name="JointCalibrationControllerNode" type="JointCalibrationControllerNode" base_class_type="Controller" />
<class name="JointChainSineController" type="JointChainSineController" base_class_type="Controller" />
<class name="JointLimitCalibrationControllerNode" type="JointLimitCalibrationControllerNode" base_class_type="Controller" />
+ <class name="JointPDController" type="JointPDController" base_class_type="Controller" />
+ <class name="JointPDControllerNode" type="JointPDControllerNode" base_class_type="Controller" />
+ <class name="CartesianTrajectoryController" type="CartesianTrajectoryController" base_class_type="Controller" />
<class name="HeadServoingController" type="HeadServoingController" base_class_type="Controller" />
<class name="JointTrajectoryController" type="JointTrajectoryController" base_class_type="Controller" />
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h 2009-08-21 19:02:00 UTC (rev 22569)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h 2009-08-21 19:05:43 UTC (rev 22570)
@@ -41,7 +41,7 @@
#include <robot_mechanism_controllers/joint_position_controller.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
#include <robot_mechanism_controllers/joint_effort_controller.h>
-#include <robot_mechanism_controllers/joint_pd_controller.h>
+#include <experimental_controllers/joint_pd_controller.h>
// Services
#include <experimental_controllers/GetJointPosCmd.h>
Copied: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_trajectory_controller.h (from rev 22565, pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_trajectory_controller.h)
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_trajectory_controller.h (rev 0)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_trajectory_controller.h 2009-08-21 19:05:43 UTC (rev 22570)
@@ -0,0 +1,113 @@
+/*
+ * 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: Wim Meeussen
+ */
+
+#ifndef CARTESIAN_TRAJECTORY_CONTROLLER_H
+#define CARTESIAN_TRAJECTORY_CONTROLLER_H
+
+#include <vector>
+#include <string>
+#include <kdl/chain.hpp>
+#include <kdl/frames.hpp>
+#include <kdl/velocityprofile_trap.hpp>
+#include <tf/transform_listener.h>
+#include <tf/message_notifier.h>
+#include <ros/node.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <controller_interface/controller.h>
+#include <robot_mechanism_controllers/cartesian_pose_controller.h>
+#include <deprecated_srvs/MoveToPose.h>
+#include <std_srvs/Empty.h>
+#include <boost/scoped_ptr.hpp>
+
+namespace controller {
+
+
+class CartesianTrajectoryController : public Controller
+{
+public:
+ CartesianTrajectoryController();
+ ~CartesianTrajectoryController();
+
+ bool init(mechanism::RobotState *robot_state, const ros::NodeHandle& n);
+
+ bool starting();
+ void update();
+ bool moveTo(const geometry_msgs::PoseStamped& pose,
+ const geometry_msgs::Twist& tolerance=geometry_msgs::Twist(), double duration=0);
+
+
+private:
+ KDL::Frame getPose();
+ void TransformToFrame(const tf::Transform& trans, KDL::Frame& frame);
+
+ // topic
+ void command(const tf::MessageNotifier<geometry_msgs::PoseStamped>::MessagePtr& pose_msg);
+
+ // service calls
+ bool moveTo(deprecated_srvs::MoveToPose::Request &req, deprecated_srvs::MoveToPose::Response &resp);
+ bool preempt(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
+
+ ros::NodeHandle node_;
+ ros::ServiceServer move_to_srv_, preempt_srv_;
+
+ std::string controller_name_;
+ double last_time_, time_started_, time_passed_, max_duration_;
+ bool is_moving_, request_preempt_, exceed_tolerance_;
+
+ KDL::Frame pose_begin_, pose_end_, pose_current_;
+ KDL::Twist twist_current_, tolerance_;
+
+ // robot structure
+ mechanism::RobotState *robot_state_;
+ mechanism::Chain chain_;
+
+ // kdl stuff for kinematics
+ KDL::Chain kdl_chain_;
+ boost::scoped_ptr<KDL::ChainFkSolverPos> jnt_to_pose_solver_;
+ KDL::JntArray jnt_pos_;
+
+ // motion profiles
+ std::vector<KDL::VelocityProfile_Trap> motion_profile_;
+
+ // pose controller
+ CartesianPoseController* pose_controller_;
+
+ tf::TransformListener tf_;
+ boost::scoped_ptr<tf::MessageNotifier<geometry_msgs::PoseStamped> > command_notifier_;
+
+ std::string root_name_;
+};
+
+
+}
+#endif
Copied: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_pd_controller.h (from rev 22565, pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_pd_controller.h)
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_pd_controller.h (rev 0)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_pd_controller.h 2009-08-21 19:05:43 UTC (rev 22570)
@@ -0,0 +1,168 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * 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 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.
+ *********************************************************************/
+
+#pragma once
+
+/***************************************************/
+/*! \class controller::JointPDController
+ \brief Joint Velocity Controller
+
+ This class closes the loop around velocity using
+ a pid loop.
+
+ Example config:<br>
+
+ <controller type="JointPDController" name="controller_name"><br>
+ <joint name="joint_to_control"><br>
+ <pid p="1.0" i="2.0" d="3.0" iClamp="4.0" /><br>
+ </joint>
+ </controller>
+*/
+/***************************************************/
+
+#include <ros/node.h>
+
+#include <controller_interface/controller.h>
+#include <control_toolbox/pid.h>
+#include "misc_utils/advertised_service_guard.h"
+#include "misc_utils/subscription_guard.h"
+
+// Services
+#include <experimental_controllers/SetPDCommand.h>
+#include <experimental_controllers/GetPDCommand.h>
+#include <deprecated_msgs/JointCmd.h>
+
+namespace controller
+{
+
+ class JointPDController : public Controller
+ {
+ public:
+
+ JointPDController();
+ ~JointPDController();
+
+ /*!
+ * \brief Functional way to initialize limits and gains.
+ * \param pid Pid gain values.
+ * \param joint_name Name of joint we want to control.
+ * \param *robot The robot.
+ */
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ bool init(mechanism::RobotState *robot, const std::string &joint_name,const control_toolbox::Pid &pid);
+
+ /*!
+ * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
+ *
+ * \param double pos Velocity command to issue
+ */
+ void setCommand(double command, double command_dot);
+
+ /*!
+ * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
+ */
+ void getCommand(deprecated_msgs::JointCmd & cmd);
+
+
+ /*!
+ * \brief Issues commands to the joint. Should be called at regular intervals
+ */
+ virtual void update();
+
+ void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
+ void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min);
+
+ std::string getJointName();
+ mechanism::JointState *joint_state_; /**< Joint we're controlling. */
+
+ /*!
+ * \brief Reset the internal PID controllers
+ */
+ void reset();
+
+ private:
+
+ mechanism::RobotState *robot_; /**< Pointer to robot structure. */
+ control_toolbox::Pid pid_controller_; /**< Internal PID controller. */
+ double last_time_; /**< Last time stamp of update. */
+ double command_; /**< Last commanded position. */
+ double command_dot_;
+ double command_t_; /**< Last commanded position. */
+ double command_dot_t_;
+
+ /*!
+ * \brief mutex lock for setting and getting commands
+ */
+ pthread_mutex_t joint_pd_controller_lock_;
+
+ };
+
+/***************************************************/
+/*! \class controller::JointPDControllerNode
+ \brief Joint PD Controller ROS Node
+
+ This class closes the loop around velocity using
+ a pid loop.
+
+*/
+/***************************************************/
+
+ class JointPDControllerNode : public Controller
+ {
+ public:
+
+ JointPDControllerNode();
+ ~JointPDControllerNode();
+
+ void update();
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+ // Topics
+ void setCommand();
+
+ private:
+
+ //node stuff
+ std::string service_prefix_; /**< The name of the controller. */
+ ros::Node *node_;
+ SubscriptionGuard guard_set_command_; /**< Makes sure the subscription goes down neatly. */
+
+ //msgs
+ deprecated_msgs::JointCmd cmd_; /**< The command from the subscription. */
+
+ //controller
+ JointPDController *c_; /**< The controller. */
+
+ };
+}
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_trajectory_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_trajectory_controller.h 2009-08-21 19:02:00 UTC (rev 22569)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_trajectory_controller.h 2009-08-21 19:05:43 UTC (rev 22570)
@@ -38,7 +38,7 @@
#include <boost/thread/mutex.hpp>
#include <controller_interface/controller.h>
-#include <robot_mechanism_controllers/joint_pd_controller.h>
+#include <experimental_controllers/joint_pd_controller.h>
// Services
#include <manipulation_msgs/JointTraj.h>
Copied: pkg/trunk/sandbox/experimental_controllers/src/cartesian_trajectory_controller.cpp (from rev 22565, pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp)
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/cartesian_trajectory_controller.cpp (rev 0)
+++ pkg/trunk/sandbox/experimental_controllers/src/cartesian_trajectory_controller.cpp 2009-08-21 19:05:43 UTC (rev 22570)
@@ -0,0 +1,350 @@
+/*
+ * 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: Wim Meeussen
+ */
+
+
+
+#include <algorithm>
+#include <mechanism_control/mechanism_control.h>
+#include "kdl/chainfksolverpos_recursive.hpp"
+#include "experimental_controllers/cartesian_trajectory_controller.h"
+#include "pluginlib/class_list_macros.h"
+
+
+using namespace KDL;
+using namespace tf;
+using namespace ros;
+
+
+PLUGINLIB_REGISTER_CLASS(CartesianTrajectoryController, controller::CartesianTrajectoryController, controller::Controller)
+
+namespace controller {
+
+ROS_REGISTER_CONTROLLER(CartesianTrajectoryController)
+
+CartesianTrajectoryController::CartesianTrajectoryController()
+: jnt_to_pose_solver_(NULL),
+ motion_profile_(6, VelocityProfile_Trap(0,0))
+{}
+
+CartesianTrajectoryController::~CartesianTrajectoryController()
+{}
+
+
+
+bool CartesianTrajectoryController::init(mechanism::RobotState *robot_state, const NodeHandle& n)
+{
+ node_ = n;
+
+ // get name of root and tip from the parameter server
+ std::string tip_name;
+ if (!node_.getParam("root_name", root_name_)){
+ ROS_ERROR("CartesianTrajectoryController: No root name found on parameter server");
+ return false;
+ }
+ if (!node_.getParam("tip_name", tip_name)){
+ ROS_ERROR("CartesianTrajectoryController: No tip name found on parameter server");
+ return false;
+ }
+
+ // test if we got robot pointer
+ assert(robot_state);
+ robot_state_ = robot_state;
+
+ // create robot chain from root to tip
+ if (!chain_.init(robot_state->model_, root_name_, tip_name))
+ return false;
+ chain_.toKDL(kdl_chain_);
+
+ // create solver
+ jnt_to_pose_solver_.reset(new ChainFkSolverPos_recursive(kdl_chain_));
+ jnt_pos_.resize(kdl_chain_.getNrOfJoints());
+
+ // initialize motion profile
+ double max_vel_trans, max_vel_rot, max_acc_trans, max_acc_rot;
+ node_.param("max_vel_trans", max_vel_trans, 0.0) ;
+ node_.param("max_vel_rot", max_vel_rot, 0.0) ;
+ node_.param("max_acc_trans", max_acc_trans, 0.0) ;
+ node_.param("max_acc_rot", max_acc_rot, 0.0) ;
+ for (unsigned int i=0; i<3; i++){
+ motion_profile_[i ].SetMax(max_vel_trans, max_acc_trans);
+ motion_profile_[i+3].SetMax(max_vel_rot, max_acc_rot);
+ }
+
+ // get a pointer to the pose controller
+ std::string output;
+ if (!node_.getParam("output", output)){
+ ROS_ERROR("CartesianTrajectoryController: No ouptut name found on parameter server");
+ return false;
+ }
+ if (!getController<CartesianPoseController>(output, AFTER_ME, pose_controller_)){
+ ROS_ERROR("CartesianTrajectoryController: could not connect to pose controller");
+ return false;
+ }
+
+ // subscribe to pose commands
+ command_notifier_.reset(new MessageNotifier<geometry_msgs::PoseStamped>(tf_,
+ boost::bind(&CartesianTrajectoryController::command, this, _1),
+ node_.getNamespace() + "/command", root_name_, 1));
+ // advertise services
+ move_to_srv_ = node_.advertiseService("move_to", &CartesianTrajectoryController::moveTo, this);
+ preempt_srv_ = node_.advertiseService("preempt", &CartesianTrajectoryController::preempt, this);
+
+ return true;
+}
+
+
+
+
+bool CartesianTrajectoryController::moveTo(const geometry_msgs::PoseStamped& pose, const geometry_msgs::Twist& tolerance, double duration)
+{
+ // don't do anything when still moving
+ if (is_moving_) return false;
+
+ // convert message to transform
+ Stamped<Pose> pose_stamped;
+ poseStampedMsgToTF(pose, pose_stamped);
+
+ // convert to reference frame of root link of the controller chain
+ Duration timeout = Duration().fromSec(2.0);
+ std::string error_msg;
+ if (!tf_.waitForTransform(root_name_, pose.header.frame_id, pose_stamped.stamp_, timeout, Duration(0.01), &error_msg)){
+ ROS_ERROR("CartesianTrajectoryController: %s", error_msg.c_str());
+ return false;
+ }
+ tf_.transformPose(root_name_, pose_stamped, pose_stamped);
+
+ // trajectory from pose_begin to pose_end
+ TransformToFrame(pose_stamped, pose_end_);
+ pose_begin_ = pose_current_;
+
+ max_duration_ = 0;
+ Twist twist_move = diff(pose_begin_, pose_end_);
+
+ // Set motion profiles
+ for (unsigned int i=0; i<6; i++){
+ motion_profile_[i].SetProfileDuration( 0, twist_move(i), duration);
+ max_duration_ = max( max_duration_, motion_profile_[i].Duration() );
+ }
+
+ // Rescale trajectories to maximal duration
+ for (unsigned int i=0; i<6; i++)
+ motion_profile_[i].SetProfileDuration( 0, twist_move(i), max_duration_ );
+
+ // set tolerance
+ tolerance_.vel(0) = tolerance.linear.x;
+ tolerance_.vel(1) = tolerance.linear.y;
+ tolerance_.vel(2) = tolerance.linear.z;
+ tolerance_.rot(0) = tolerance.angular.x;
+ tolerance_.rot(1) = tolerance.angular.y;
+ tolerance_.rot(2) = tolerance.angular.z;
+
+ time_passed_ = 0;
+
+ exceed_tolerance_ = false;
+ request_preempt_ = false;
+ is_moving_ = true;
+
+ ROS_INFO("CartesianTrajectoryController: %s will move to new pose in %f seconds", controller_name_.c_str(), max_duration_);
+
+ return true;
+}
+
+
+
+bool CartesianTrajectoryController::starting()
+{
+ // time
+ last_time_ = robot_state_->hw_->current_time_;
+
+ // set desired pose to current pose
+ pose_current_ = getPose();
+ twist_current_ = Twist::Zero();
+
+ // start not moving
+ is_moving_ = false;
+ request_preempt_ = false;
+
+ return true;
+}
+
+
+
+
+void CartesianTrajectoryController::update()
+{
+ // get time
+ double time = robot_state_->hw_->current_time_;
+ double dt = time - last_time_;
+ last_time_ = time;
+
+ // preempt trajectory
+ if (request_preempt_){
+ twist_current_ = Twist::Zero();
+ is_moving_ = false;
+ }
+
+ // if we are moving
+ if (is_moving_){
+ time_passed_ += dt;
+
+ // check tolerance
+ for (unsigned int i=0; i<6; i++){
+ if (tolerance_[i] != 0 && pose_controller_->twist_error_[i] > tolerance_[i]){
+ exceed_tolerance_ = true;
+ is_moving_ = false;
+ }
+ }
+
+ // ended trajectory
+ if (time_passed_ > max_duration_){
+ twist_current_ = Twist::Zero();
+ pose_current_ = pose_end_;
+ is_moving_ = false;
+ }
+ // still in trajectory
+ else{
+ // pose
+ Twist twist_begin_current = Twist(Vector(motion_profile_[0].Pos(time_passed_),
+ motion_profile_[1].Pos(time_passed_),
+ motion_profile_[2].Pos(time_passed_)),
+ Vector(motion_profile_[3].Pos(time_passed_),
+ motion_profile_[4].Pos(time_passed_),
+ motion_profile_[5].Pos(time_passed_)) );
+ pose_current_ = Frame( pose_begin_.M * Rot( pose_begin_.M.Inverse( twist_begin_current.rot ) ),
+ pose_begin_.p + twist_begin_current.vel);
+
+ // twist
+ for(unsigned int i=0; i<6; i++)
+ twist_current_(i) = motion_profile_[i].Vel( time_passed_ );
+ }
+ }
+
+ // send output to pose controller
+ pose_controller_->pose_desi_ = pose_current_;
+ pose_controller_->twist_ff_ = twist_current_;
+}
+
+
+
+Frame CartesianTrajectoryController::getPose()
+{
+ // get the joint positions and velocities
+ chain_.getPositions(robot_state_->joint_states_, jnt_pos_);
+
+ // get cartesian pose
+ Frame result;
+ jnt_to_pose_solver_->JntToCart(jnt_pos_, result);
+
+ return result;
+}
+
+
+bool CartesianTrajectoryController::moveTo(deprecated_srvs::MoveToPose::Request &req,
+ deprecated_srvs::MoveToPose::Response &resp)
+{
+ ROS_DEBUG("in cartesian traj move_to service");
+
+ if (!moveTo(req.pose, req.tolerance, 0.0)){
+ ROS_ERROR("CartesianTrajectoryController: not starting trajectory because either previous one is still running or the transform frame could not be found");
+ return false;
+ }
+
+ ros::Duration timeout = Duration().fromSec(3.0);
+ ros::Duration traj_duration = Duration().fromSec(max_duration_);
+ ros::Time start_time = ros::Time::now();
+
+ while (is_moving_){
+ Duration().fromSec(0.1).sleep();
+ if (ros::Time::now() > start_time + timeout + traj_duration){
+ ROS_ERROR("CartesianTrajectoryController: timeout");
+ return false;
+ }
+ }
+
+
+ if (request_preempt_){
+ ROS_ERROR("CartesianTrajectoryController: trajectory preempted");
+ return false;
+ }
+ else if (exceed_tolerance_){
+ ROS_ERROR("CartesianTrajectoryController: exceeded trajectory tolerance");
+ return false;
+ }
+ else{
+ ROS_DEBUG("CartesianTrajectoryCon...
[truncated message content] |
