Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22045] pkg/trunk
|
From: <stu...@us...> - 2009-08-17 22:40:13
|
Revision: 22045
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22045&view=rev
Author: stuglaser
Date: 2009-08-17 22:39:59 +0000 (Mon, 17 Aug 2009)
Log Message:
-----------
Moved controllers that won't be released out of robot_mechanism_controllers
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp
pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt
pkg/trunk/sandbox/experimental_controllers/src/controller_manifest.cpp
Added Paths:
-----------
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_hybrid_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_tff_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_twist_controller_ik.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/dynamic_loader_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/endeffector_constraint_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_autotuner.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_blind_calibration_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_calibration_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_constraint_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_sine_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_inverse_dynamics_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_limit_calibration_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_position_smoothing_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/probe.h
pkg/trunk/sandbox/experimental_controllers/msg/CartesianHybridState.msg
pkg/trunk/sandbox/experimental_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_tff_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_twist_controller_ik.cpp
pkg/trunk/sandbox/experimental_controllers/src/dynamic_loader_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_autotuner.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_blind_calibration_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_calibration_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_chain_constraint_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_chain_sine_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_inverse_dynamics_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_limit_calibration_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_position_smoothing_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/probe.cpp
Removed Paths:
-------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_tff_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/dynamic_loader_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_autotuner.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_blind_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_sine_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_inverse_dynamics_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_limit_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_smoothing_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/probe.h
pkg/trunk/controllers/robot_mechanism_controllers/msg/CartesianHybridState.msg
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_tff_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller_ik.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/dynamic_loader_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_autotuner.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_blind_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_chain_constraint_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_chain_sine_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_inverse_dynamics_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_limit_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_smoothing_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/probe.cpp
Modified: pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-08-17 22:39:59 UTC (rev 22045)
@@ -7,35 +7,19 @@
gensrv()
set(_srcs
src/controller_manifest.cpp
- src/cartesian_tff_controller.cpp
src/cartesian_trajectory_controller.cpp
src/cartesian_pose_controller.cpp
src/cartesian_twist_controller.cpp
- src/cartesian_twist_controller_ik.cpp
src/cartesian_wrench_controller.cpp
- src/cartesian_hybrid_controller.cpp
- src/dynamic_loader_controller.cpp
src/joint_effort_controller.cpp
src/joint_position_controller.cpp
- src/joint_position_smoothing_controller.cpp
src/joint_velocity_controller.cpp
- src/joint_inverse_dynamics_controller.cpp
- src/joint_autotuner.cpp
src/joint_pd_controller.cpp
- src/joint_calibration_controller.cpp
src/joint_ud_calibration_controller.cpp
- src/joint_limit_calibration_controller.cpp
- src/joint_blind_calibration_controller.cpp
- src/joint_chain_constraint_controller.cpp
- src/joint_chain_sine_controller.cpp
src/trigger_controller.cpp
- src/probe.cpp
)
# For some reason, the endeffector_constraint_controller won't build on OSX
-if(NOT APPLE)
- set(_srcs ${_srcs} src/endeffector_constraint_controller.cpp)
-endif(NOT APPLE)
rospack_add_library(robot_mechanism_controllers ${_srcs})
rospack_remove_compile_flags(robot_mechanism_controllers -W)
target_link_libraries(robot_mechanism_controllers ltdl)
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h 2009-08-17 22:39:59 UTC (rev 22045)
@@ -1,135 +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
-
-#include "ros/node.h"
-#include "boost/scoped_ptr.hpp"
-#include "controller_interface/controller.h"
-#include <kdl/chain.hpp>
-#include <kdl/frames.hpp>
-#include "mechanism_model/chain.h"
-#include "control_toolbox/pid.h"
-#include <tf/transform_listener.h>
-#include <tf/message_notifier.h>
-#include "realtime_tools/realtime_publisher.h"
-#include "filters/filter_chain.h"
-#include "control_toolbox/pid_gains_setter.h"
-
-#include "robot_mechanism_controllers/SetPoseStamped.h"
-#include "manipulation_msgs/TaskFrameFormalism.h"
-#include "robot_mechanism_controllers/CartesianHybridState.h"
-
-namespace controller {
-
-class CartesianHybridController : public Controller
-{
-public:
- CartesianHybridController();
- ~CartesianHybridController() {}
-
- virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- virtual bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
- virtual void update(void);
- virtual bool starting();
-
- KDL::Twist pose_error_;
- KDL::Twist twist_error_;
-
- KDL::Twist pose_desi_;
- KDL::Twist twist_desi_;
- KDL::Wrench wrench_desi_;
- KDL::Frame pose_meas_;
- KDL::Twist twist_meas_;
- KDL::Twist twist_meas_filtered_;
- std::vector<double> measured_torque_, desired_torque_, max_jnt_eff_;
-
- control_toolbox::Pid pose_pids_[6]; // (x,y,z) position, then (x,y,z) rot
- control_toolbox::Pid twist_pids_[6];
- control_toolbox::PidGainsSetter pose_pid_tuner_;
- control_toolbox::PidGainsSetter pose_rot_pid_tuner_;
- control_toolbox::PidGainsSetter twist_pid_tuner_;
- control_toolbox::PidGainsSetter twist_rot_pid_tuner_;
-
- // x, y, z, rx, ry, rz
- int mode_[6];
- double setpoint_[6];
-
- KDL::Frame task_frame_offset_; // task frame in the root frame
- KDL::Frame tool_frame_offset_; // tool frame in the ee frame
-
- mechanism::Chain chain_;
- KDL::Chain kdl_chain_;
- mechanism::RobotState *robot_;
- double last_time_;
-
- int initial_mode_;
-
- double saturated_velocity_, saturated_rot_velocity_;
-
- bool use_filter_;
- filters::FilterChain<double> twist_filter_;
-
- ros::NodeHandle node_;
-};
-
-class CartesianHybridControllerNode : public Controller
-{
-public:
- CartesianHybridControllerNode();
- ~CartesianHybridControllerNode();
-
- virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- virtual bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
- virtual void update(void);
- virtual bool starting() { return c_.starting(); }
-
- void command(const tf::MessageNotifier<manipulation_msgs::TaskFrameFormalism>::MessagePtr& tff_msg);
-
- bool setToolFrame(robot_mechanism_controllers::SetPoseStamped::Request &req,
- robot_mechanism_controllers::SetPoseStamped::Response &resp);
-
-private:
- ros::NodeHandle node_;
- ros::ServiceServer serve_set_tool_frame_;
- std::string name_;
- tf::TransformListener TF;
- CartesianHybridController c_;
- //manipulation_msgs::TaskFrameFormalism command_msg_;
- boost::scoped_ptr<tf::MessageNotifier<manipulation_msgs::TaskFrameFormalism> > command_notifier_;
-
- std::string task_frame_name_;
-
- unsigned int loop_count_;
- boost::scoped_ptr<realtime_tools::RealtimePublisher<robot_mechanism_controllers::CartesianHybridState> > pub_state_;
- boost::scoped_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > pub_tf_;
-};
-
-}
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_tff_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_tff_controller.h 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_tff_controller.h 2009-08-17 22:39:59 UTC (rev 22045)
@@ -1,103 +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_TFF_CONTROLLER_H
-#define CARTESIAN_TFF_CONTROLLER_H
-
-#include <vector>
-#include <kdl/chain.hpp>
-#include <kdl/chainfksolver.hpp>
-#include <kdl/frames.hpp>
-#include <ros/node.h>
-#include <manipulation_msgs/TaskFrameFormalism.h>
-#include <geometry_msgs/Twist.h>
-#include <controller_interface/controller.h>
-#include <tf/transform_datatypes.h>
-#include <control_toolbox/pid.h>
-#include <boost/scoped_ptr.hpp>
-#include "robot_mechanism_controllers/cartesian_wrench_controller.h"
-#include "robot_mechanism_controllers/joint_chain_constraint_controller.h"
-
-namespace controller {
-
-class CartesianTFFController : public Controller
-{
-public:
- CartesianTFFController();
- ~CartesianTFFController();
-
- bool init(mechanism::RobotState *robot_state, const ros::NodeHandle& n);
- bool starting();
- void update();
-
- void command(const manipulation_msgs::TaskFrameFormalismConstPtr& tff_msg);
-
-private:
- ros::NodeHandle node_;
- ros::Subscriber sub_command_;
- double last_time_;
-
- // pid controllers
- std::vector<control_toolbox::Pid> vel_pid_controller_, pos_pid_controller_;
-
- // robot description
- mechanism::RobotState *robot_state_;
- mechanism::Chain chain_;
-
- // kdl stuff for kinematics
- KDL::Chain kdl_chain_;
- boost::scoped_ptr<KDL::ChainFkSolverVel> jnt_to_twist_solver_;
- KDL::JntArrayVel jnt_posvel_;
-
- // command for tff
- std::vector<int> mode_;
- std::vector<double> value_, twist_to_wrench_;
-
- // output of the controller
- KDL::Wrench wrench_desi_;
-
- KDL::Twist position_, twist_meas_;
- KDL::Frame pose_meas_, pose_meas_old_;
-
- boost::scoped_ptr<realtime_tools::RealtimePublisher<geometry_msgs::Twist> > state_position_publisher_;
- unsigned int loop_count_;
-
- // internal wrench controller
- CartesianWrenchController* wrench_controller_;
-};
-
-
-} // namespace
-
-
-#endif
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h 2009-08-17 22:39:59 UTC (rev 22045)
@@ -1,97 +0,0 @@
-/*
- * Copyright (c) 2008, Ruben Smits and 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
- * Ruben Smits
- */
-
-#ifndef CARTESIAN_TWIST_CONTROLLER_IK_H
-#define CARTESIAN_TWIST_CONTROLLER_IK_H
-
-#include <vector>
-#include <boost/scoped_ptr.hpp>
-#include <kdl/chain.hpp>
-#include <kdl/frames.hpp>
-#include <kdl/chainfksolver.hpp>
-#include <kdl/chainiksolver.hpp>
-#include <ros/node.h>
-#include <geometry_msgs/Twist.h>
-#include <controller_interface/controller.h>
-#include <mechanism_model/chain.h>
-#include <tf/transform_datatypes.h>
-#include <control_toolbox/pid.h>
-
-#include "robot_mechanism_controllers/joint_inverse_dynamics_controller.h"
-
-namespace controller {
-
- class CartesianTwistControllerIk : public Controller
- {
- public:
- CartesianTwistControllerIk();
- ~CartesianTwistControllerIk();
-
- bool init(mechanism::RobotState *robot_state, const ros::NodeHandle& n);
- bool starting();
- void update();
-
- void command(const geometry_msgs::TwistConstPtr& twist_msg);
-
- // input of the controller
- KDL::Twist twist_desi_;
-
- private:
- KDL::Twist twist_meas_,error,twist_out_;
-
- ros::NodeHandle node_;
- ros::Subscriber sub_command_;
-
- double last_time_,ff_trans_,ff_rot_;
-
- // pid controllers
- std::vector<control_toolbox::Pid> fb_pid_controller_;
-
- // robot description
- mechanism::RobotState *robot_state_;
- mechanism::Chain chain_;
-
- // kdl stuff for kinematics
- KDL::Chain kdl_chain_;
- boost::scoped_ptr<KDL::ChainFkSolverVel> jnt_to_twist_solver_;
- boost::scoped_ptr<KDL::ChainIkSolverVel> twist_to_jnt_solver_;
- KDL::JntArrayVel jnt_posvel_;
-
- JointInverseDynamicsController* id_controller_;
- };
-
-} // namespace
-
-
-#endif
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/dynamic_loader_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/dynamic_loader_controller.h 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/dynamic_loader_controller.h 2009-08-17 22:39:59 UTC (rev 22045)
@@ -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: Rob Wheeler
- */
-
-#ifndef DYNAMIC_LOADER_CONTROLLER_H
-#define DYNAMIC_LOADER_CONTROLLER_H
-
-#include "controller_interface/controller.h"
-#include <tinyxml/tinyxml.h>
-#include <ltdl.h>
-
-namespace controller {
-
-/***************************************************/
-/*! \class controller::DynamicLoaderController
- \brief Dynamic Loader
-
- This class implements a pseudo-controller that can dynamically
- load a package's shared object and instantiate controllers from
- that shared object.
-
- When the DynamicLoaderController is killed, it shuts down the
- controllers that it started and unloads the shared object.
-
- Example configuration:
- <pre>
- <controller name="dynamic_loader" type="DynamicLoaderController"
- package="my_controllers" lib="libmy_controllers">
-
- <controllers>
-
- <controller type="MyController" name="my_controller1">
- <joint name="joint_to_control" />
- </controller><br>
-
- <controller type="MyController" name="my_controller2">
- <joint name="another_joint_to_control" />
- </controller><br>
-
- </controllers>
-
- </controller>
- </pre>
-
- The above example creates an instance of the DynamicLoaderController
- that loads the shared object libmy_controllers.so from the
- my_controllers package. It then instantiates two controllers,
- my_controller1 and my_controller2, from that shared object.
- When the DynamicLoaderController is killed, it will kill the two
- controllers it started and unload the libmy_controllers.so shared object.
-
-*/
-/***************************************************/
-
-class DynamicLoaderController : public Controller
-{
-public:
- DynamicLoaderController();
- ~DynamicLoaderController();
-
- /*!
- * \brief Specifies the package and shared object to load.
- * \param *robot The robot (not used by this controller).
- * \param *config The XML configuration for this controller
- */
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- /*!
- * \brief This function is called in the control loop. For this
- * pseudo-controller, the update function is a noop.
- */
- void update();
-
-private:
- std::vector<std::string> names_;
- lt_dlhandle handle_;
-
- void loadLibrary(std::string& name);
- static void unloadLibrary(std::vector<std::string> names, lt_dlhandle handle);
-};
-
-}
-
-#endif
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-08-17 22:39:59 UTC (rev 22045)
@@ -1,143 +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: Melonee Wise
- */
-
-#ifndef ENDEFFECTOR_CONSTRAINT_CONTROLLER_H
-#define ENDEFFECTOR_CONSTRAINT_CONTROLLER_H
-
-#include <vector>
-#include "boost/scoped_ptr.hpp"
-#include "mechanism_model/chain.h"
-#include "kdl/chain.hpp"
-#include "kdl/frames.hpp"
-#include "kdl/chainfksolver.hpp"
-#include "kdl/chainjnttojacsolver.hpp"
-#include "ros/node.h"
-#include "geometry_msgs/Wrench.h"
-#include "misc_utils/subscription_guard.h"
-#include "controller_interface/controller.h"
-#include "tf/transform_datatypes.h"
-#include "misc_utils/advertised_service_guard.h"
-#include "joy/Joy.h"
-#include "Eigen/LU"
-#include "Eigen/Core"
-#include <visualization_msgs/Marker.h>
-
-
-namespace controller {
-
-class EndeffectorConstraintController : public Controller
-{
-public:
- EndeffectorConstraintController();
- ~EndeffectorConstraintController();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- void update();
- void computeConstraintJacobian();
- void computeConstraintNullSpace();
- // input of the controller
- KDL::Wrench wrench_desi_;
- Eigen::Matrix<float,6,1> task_wrench_;
-
-private:
-
- mechanism::RobotState *robot_;
-
- // kdl stuff for kinematics
- mechanism::Chain chain_;
- KDL::Chain kdl_chain_;
- boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;
- boost::scoped_ptr<KDL::ChainFkSolverPos> jnt_to_pose_solver_;
-
- // to get joint positions, velocities, and to set joint torques
- Eigen::Matrix<float,6,5> constraint_jac_;
- Eigen::Matrix<float,6,1> constraint_wrench_;
- Eigen::Matrix<float,5,1> constraint_force_;
- // joint constraint
- Eigen::MatrixXf joint_constraint_force_;
- Eigen::MatrixXf joint_constraint_jac_;
- Eigen::MatrixXf joint_constraint_null_space_;
-
- Eigen::MatrixXf task_jac_;
- Eigen::MatrixXf identity_;
- Eigen::MatrixXf identity_joint_;
- Eigen::MatrixXf constraint_null_space_;
- Eigen::MatrixXf constraint_torq_;
- Eigen::MatrixXf joint_constraint_torq_;
- Eigen::MatrixXf task_torq_;
- KDL::Frame endeffector_frame_;
- KDL::Frame desired_frame_;
-
- // some parameters to define the constraint
- double wall_x;
- double elbow_limit;
- double threshold_x;
- double wall_r;
- double threshold_r;
- double f_x_max;
- double f_r_max;
- double f_pose_max;
- double f_limit_max;
-
- double desired_roll_;
- double desired_pitch_;
- double desired_yaw_;
- bool initialized_;
-};
-
-
-class EndeffectorConstraintControllerNode : public Controller
-{
- public:
- EndeffectorConstraintControllerNode();
- ~EndeffectorConstraintControllerNode();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- void update();
- void command();
-
- private:
- std::string topic_;
- ros::Node *node_;
- EndeffectorConstraintController controller_;
- SubscriptionGuard guard_command_;
-
- geometry_msgs::Wrench wrench_msg_;
-
- unsigned int loop_count_;
-};
-
-} // namespace
-
-
-#endif
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_autotuner.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_autotuner.h 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_autotuner.h 2009-08-17 22:39:59 UTC (rev 22045)
@@ -1,195 +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::JointAutotuner
- \brief Joint Position Controller
-
- This class closes the loop around positon using
- a pid loop.
-
-*/
-/***************************************************/
-
-#include <ros/node.h>
-#include <vector>
-#include <controller_interface/controller.h>
-#include <control_toolbox/pid.h>
-
-// Services
-#include <robot_mechanism_controllers/SetCommand.h>
-#include <robot_mechanism_controllers/GetActual.h>
-
-namespace controller
-{
-
-class JointAutotuner : public Controller
-{
- enum AutoControlState
- {
- START,POSITIVE_PEAK,NEGATIVE_PEAK, DONE, MANUAL
- };
-public:
- /*!
- * \brief Default Constructor of the JointAutotuner class.
- *
- */
- JointAutotuner();
-
- /*!
- * \brief Destructor of the JointAutotuner class.
- */
- ~JointAutotuner();
-
- /*!
- * \brief Functional way to initialize limits and gains.
- * \param p_gain Proportional gain.
- * \param i_gain Integral gain.
- * \param d_gain Derivative gain.
- * \param windup Intergral limit.
- * \param time The current hardware time.
- * \param *joint The joint that is being controlled.
- */
- void init(double p_gain, double i_gain, double d_gain, double windup, double time,mechanism::Robot *robot, mechanism::Joint *joint);
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- /*!
- * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
- *
- * \param command
- */
- void setCommand(double command);
-
- /*!
- * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
- */
- double getCommand();
-
- /*!
- * \brief Read the torque of the motor
- */
- double getMeasuredState();
-
- /*!
- * \brief Get latest time..
- */
- double getTime();
-
-
- /*!
- * \brief Issues commands to the joint. Should be called at regular intervals
- */
-
- virtual void update();
-
- double p_gain_;
- double i_gain_;
- double d_gain_;
-
- AutoControlState current_state_;
-
-private:
- bool tune_velocity_; /**<If true, uses velocity to tune. Otherwise uses position>*/
- mechanism::Joint* joint_; /**< Joint we're controlling.> */
- mechanism::JointState* joint_state_; /**< Joint we're controlling.> */
- control_toolbox::Pid pid_controller_; /**< Internal PID controller.> */
- double last_time_; /**< Last time stamp of update.> */
- double command_; /**< Last commanded position.> */
- mechanism::Robot *robot_; /**< Pointer to robot structure.> */
- mechanism::RobotState *robot_state_; /**< Pointer to robot structure.> */
- const char* file_path_; /**<Filename and location to write results. >*/
- void writeGainValues(double period, double amplitude, double relay_height); /**<Calculate and write gain values> */
-
- double amplitude_; /**< Current amplitude of relay cycle> */
- double last_amplitude_;/**< Last amplitude of relay cycle> */
- double period_;/**< Current period of relay cycle> */
- double last_period_;/**< Last period of relay cycle> */
-
- double positive_peak_;/**< Positive peak reached in cycle> */
- double negative_peak_;/**< Negative peak reached in cycle> */
-
- double relay_height_;/**< Amount of relay input> */
- int successful_cycles_;/**< Number of matching cycles > */
- double crossing_point_;/**< Location of crossover point for relay test> */
- double cycle_start_time_;/**< Mark time of cycle start> */
-
- int num_cycles_; /*!<Number of cycles that need to match for autotuner to read as stable>!*/
- double amplitude_tolerance_; /*!<% variation amplitude allowed between successful cycles>!*/
- double period_tolerance_; /*!<% variation period allowed between successful cycles>!*/
- double relay_effort_percent_; /*!<% of effort limit to use in relay test>!*/
-
-};
-
-/***************************************************/
-/*! \class controller::JointAutotunerNode
- \brief Joint Position Controller ROS Node
-
- This class performs an autotuning routine using the relay method.
-
-
-*/
-/***************************************************/
-
-class JointAutotunerNode : public Controller
-{
-public:
- /*!
- * \brief Default Constructor
- *
- */
- JointAutotunerNode();
-
- /*!
- * \brief Destructor
- */
- ~JointAutotunerNode();
-
- void update();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- // Services
- bool setCommand(robot_mechanism_controllers::SetCommand::Request &req,
- robot_mechanism_controllers::SetCommand::Response &resp);
-
- bool getActual(robot_mechanism_controllers::GetActual::Request &req,
- robot_mechanism_controllers::GetActual::Response &resp);
-
-private:
- JointAutotuner *c_;
-};
-}
-
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_blind_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_blind_calibration_controller.h 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_blind_calibration_controller.h 2009-08-17 22:39:59 UTC (rev 22045)
@@ -1,156 +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::JointCalibratonController
- \brief Joint Controller that finds zerop point
- \author Timothy Hunter <tjh...@wi...>
-
-
- This class moves the joint and reads the value of the clibration_reading_ field to find the zero position of the joint. Once these are determined, these values
- * are passed to the joint and enable the joint for the other controllers.
-
- Example XML:
- <controller type="JointBlindCalibrationController">
- <calibrate joint="upperarm_roll_right_joint"
- actuator="upperarm_roll_right_act"
- transmission="upperarm_roll_right_trans"
- velocity="0.3" />
- <pid p="15" i="0" d="0" iClamp="0" />
- </controller>
-
-*/
-/***************************************************/
-
-
-#include "controller_interface/controller.h"
-#include "robot_mechanism_controllers/joint_velocity_controller.h"
-#include "misc_utils/advertised_service_guard.h"
-
-// Services
-#include <robot_mechanism_controllers/CalibrateJoint.h>
-
-
-namespace controller
-{
-
-class JointBlindCalibrationController : public Controller
-{
-public:
- /*!
- * \brief Default Constructor.
- *
- */
- JointBlindCalibrationController();
-
- /*!
- * \brief Destructor.
- */
- virtual ~JointBlindCalibrationController();
-
- /*!
- * \brief Functional way to initialize limits and gains.
- *
- */
- virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
-
- /*!
- * \brief Issues commands to the joint. Should be called at regular intervals
- */
- virtual void update();
-
- bool calibrated() { return state_ == DONE; }
- void beginCalibration() {
- if (state_ == INITIALIZED)
- state_ = BEGINNING;
- }
-
-protected:
-
- enum { INITIALIZED, BEGINNING, STARTING_UP, MOVING_UP,
- STARTING_DOWN, MOVING_DOWN, BACKING_OFF, DONE };
- int state_;
-
- double search_velocity_;
- Actuator *actuator_;
- mechanism::JointState *joint_;
- mechanism::Transmission *transmission_;
-
- double init_time;
-
- controller::JointVelocityController vc_; /** The joint velocity controller used to sweep the joint.*/
-};
-
-
-/***************************************************/
-/*! \class controller::JointBlindCalibrationControllerNode
- \brief Joint Limit Controller ROS Node
-
- This class starts and stops the initialization sequence
-
-*/
-/***************************************************/
-
-class JointBlindCalibrationControllerNode : public Controller
-{
-public:
- /*!
- * \brief Default Constructor
- *
- */
- JointBlindCalibrationControllerNode();
-
- /*!
- * \brief Destructor
- */
- ~JointBlindCalibrationControllerNode();
-
- void update();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- // Services
- bool calibrateCommand(robot_mechanism_controllers::CalibrateJoint::Request &req,
- robot_mechanism_controllers::CalibrateJoint::Response &resp);
-
-private:
- JointBlindCalibrationController *c_;
- AdvertisedServiceGuard guard_calibrate_;
-};
-}
-
-
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h 2009-08-17 22:39:59 UTC (rev 22045)
@@ -1,123 +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::JointCalibratonController
- \brief Joint Controller that finds zerop point
- \author Timothy Hunter <tjh...@wi...>
-
-
- This class moves the joint and reads the value of the clibration_reading_ field to find the zero position of the joint. Once these are determined, these values
- * are passed to the joint and enable the joint for the other controllers.
-
- Example XML:
- <controller type="JointCalibrationController">
- <calibrate joint="upperarm_roll_right_joint"
- actuator="upperarm_roll_right_act"
- transmission="upperarm_roll_right_trans"
- velocity="0.3" />
- <pid p="15" i="0" d="0" iClamp="0" />
- </controller>
-
-
-
-*/
-/***************************************************/
-
-
-#include "mechanism_model/robot.h"
-#include "robot_mechanism_controllers/joint_velocity_controller.h"
-#include "realtime_tools/realtime_publisher.h"
-#include "std_msgs/Empty.h"
-
-
-namespace controller
-{
-
-class JointCalibrationController : public controller::Controller
-{
-public:
- JointCalibrationController();
- virtual ~JointCalibrationController();
-
- virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- virtual void update();
-
- bool calibrated() { return state_ == CALIBRATED; }
- void beginCalibration()
- {
- if (state_ == INITIALIZED)
- state_ = BEGINNING;
- }
-
-protected:
-
- enum { INITIALIZED, BEGINNING, MOVING, CALIBRATED };
- int state_;
-
- double search_velocity_;
- bool original_switch_state_;
-
- Actuator *actuator_;
- mechanism::JointState *joint_;
- mechanism::Transmission *transmission_;
-
- controller::JointVelocityController vc_;
-};
-
-
-class JointCalibrationControllerNode : public Controller
-{
-public:
- JointCalibrationControllerNode();
- ~JointCalibrationControllerNode();
-
- void update();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
-private:
- mechanism::RobotState* robot_;
- JointCalibrationController c_;
-
- double last_publish_time_;
- realtime_tools::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
-};
-
-}
-
-
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h 2009-08-17 22:39:59 UTC (rev 22045)
@@ -1,184 +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: Melonee Wise
- */
-
-
-/***************************************************/
-/*! \class controller::JointChainConstraintController
- \brief Adds joint position constraints to any connected chain of joints.
-
- Wrenches are in the cartesian frame in the root frame on the tip frame.
-
- Example config:<br>
-
- <controller type="JointChainConstraintController" name="controller_name"><br>
- <chain root="torso_lift_link" tip="r_gripper_tool_frame" offset="0 0 0" /><br>
- </controller><br>
-*/
-/***************************************************/
-
-
-#ifndef JOINT_CHAIN_CONSTRAINT_CONTROLLER_H
-#define JOINT_CHAIN_CONSTRAINT_CONTROLLER_H
-
-#include <vector>
-#include <cstdio>
-#include <cstring>
-#include <string>
-#include <kdl/chain.hpp>
-#include <kdl/frames.hpp>
-#include <kdl/chainjnttojacsolver.hpp>
-#include "ros/node.h"
-#include "control_toolbox/pid.h"
-#include "geometry_msgs/Wrench.h"
-#include "robot_mechanism_controllers/ChangeConstraints.h"
-#include "robot_mechanism_controllers/JointConstraint.h"
-#include "controller_interface/controller.h"
-#include "mechanism_model/chain.h"
-
-#include "tf/transform_datatypes.h"
-#include "misc_utils/advertised_service_guard.h"
-
-#include "Eigen/Geometry"
-#include "Eigen/LU"
-#include "Eigen/Core"
-
-
-namespace controller {
-
-class ConstraintState
-{
-public:
- std::string id_;
- std::string joint_name_;
- double threshold_start_;
- double nullspace_start_;
- double max_constraint_torque_;
- mechanism::Joint* joint_;
- int joint_chain_index_;
- double joint_error_;
- bool remove_;
-
- control_toolbox::Pid pid_;
-};
-
-
-class JointChainConstraintController
-{
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
-
- JointChainConstraintController();
- ~JointChainConstraintController();
-
- /*!
- * \brief Functional way to initialize the chain and construct jacobians
- * \param root_name The root of the chain.
- * \param tip_name The tip of the chain.
- * \param *robot The robot.
- */
- bool init(mechanism::RobotState *robot, const std::string& root_name,
- const std::string& tip_name, const std::string& controller_name);
-
- void update();
-
- // input of the controller
- KDL::Wrench wrench_desired_;
-
-
-private:
- void computeConstraintTorques();
- void computeConstraintJacobian();
- void computeConstraintNullSpace();
- bool addConstraint(robot_mechanism_controllers::ChangeConstraints::Request &req,
- robot_mechanism_controllers::ChangeConstraints::Response &resp);
-
- ros::Node* node_;
- Eigen::Matrix<float,6,1> task_wrench_;
- std::list<ConstraintState> constraint_list_;
- int list_size_;
- mechanism::Chain mechanism_chain_;
-
- std::string controller_name_;
- mechanism::RobotState *robot_state_;
-
- KDL::Chain kdl_chain_;
- KDL::ChainJntToJacSolver *jnt_to_jac_solver_;
- KDL::JntArray jnt_pos_, jnt_eff_;
- KDL::Jacobian chain_kdl_jacobian_;
-
- Eigen::MatrixXf identity_;
- Eigen::MatrixXf chain_eigen_jacobian_;
- Eigen::MatrixXf task_torque_;
-
- // joint constraint matrices and vectors
- Eigen::MatrixXf joint_constraint_torque_;
- Eigen::MatrixXf joint_constraint_jacobian_;
- Eigen::MatrixXf joint_constraint_null_space_;
-
- double last_time_;
-
- bool initialized_;
-
- AdvertisedServiceGuard change_constraints_guard_;
-
-};
-
-
-
-class JointChainConstraintControllerNode : public Controller
-{
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- JointChainConstraintControllerNode();
- ~JointChainConstraintControllerNode();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- void update();
-
- private:
- void command();
-
- ros::Node* node_;
- std::string controller_name_;
- JointChainConstraintController controller_;
- mechanism::RobotState *robot_;
-
- geometry_msgs::Wrench wrench_msg_;
-
-
-};
-
-} // namespace
-
-
-#endif
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_sine_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_sine_controller.h 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_sine_controller.h 2009-08-17 22:39:59 UTC (rev 22045)
@@ -1,127 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2009, 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.
- *********************************************************************/
-
-/** \author Mrinal Kalakrishnan */
-
-/**
- * \class controller::JointChainSineController
- * \brief Runs sinusoids through all joints in a kinematic chain using a PID controller per joint
- *
- * This is useful as a "random" trajectory generator to collect data for
- * dynamics parameter estimation. It also allows the definition of cartesian
- * cuboids relative to the root that are a "no-go" zone for the tip, as a
- * rudimentary way to avoid self-collisions.
- *
- * Example configuration:<br>
- *
- * <controller type="JointChainSineController" name="controller_name"><br>
- * <chain root="torso_lift_link" tip="l_gripper_tool_frame" min_freq="0.4" max_freq="1.0" freq_randomness="0.01"/><br>
- * <cartesian_constraint min="-10 -10 -10 max="10 10 -0.0" /><br>
- * <cartesian_constraint min="-5 -2 -3" max="3 4 5" /><br>
- * <joint name="l_forearm_roll_link"><br>
- * <pid p="1.0" i="0.0" d="3.0" iClamp="0.0" /><br>
- * </joint><br>
- * </controller><br>
- */
-
-#ifndef JOINT_CHAIN_SINE_CONTROLLER_H_
-#define JOINT_CHAIN_SINE_CONTROLLER_H_
-
-#include <controller_interface/controller.h>
-#include <kdl/chain.hpp>
-#include <kdl/chainfksolver.hpp>
-#include <mechanism_model/chain.h>
-#include <string>
-#include <vector>
-#include <control_toolbox/sinusoid.h>
-#include <control_toolbox/pid.h>
-#include <boost/scoped_ptr.hpp>
-#include <Eigen/Core>
-
-namespace controller
-{
-
-class JointChainSineController: public controller::Controller
-{
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- JointChainSineController();
- virtual ~JointChainSineController();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- /**
- * \brief Issues commands to the joints. Should be called at regular intervals
- */
- virtual void update();
-
-private:
- mechanism::Chain mechanism_chain_; /**< Kinematic chain */
- mechanism::RobotState *robot_state_; /**< Pointer to robot structure. */
- double last_time_; /**< Last time stamp of update. */
- bool initialized_; /**< Flag which indicates whether this class has been initialized */
- KDL::Chain kdl_chain_; /**< KDL chain */
- KDL::JntArrayVel jnt_pos_vel_; /**< Joint positions and velocities */
- KDL::JntArrayVel jnt_des_pos_vel_; /**< Joint desired positions and velocities */
- KDL::JntArray jnt_eff_; /**< Joint efforts */
- KDL::JntArray jnt_prev_des_pos_; /**< Previous desired joint positions */
- std::string controller_name_; /**< Controller name */
- int num_joints_; /**< Number of joints in the chain */
- std::vector<std::vector<control_toolbox::Sinusoid> > sinusoids_; /**< Vector of sinusoids for each joint */
- std::vector<control_toolbox::Pid> pid_controllers_; /**< A PID controller for each joint */
- boost::scoped_ptr<KDL::ChainFkSolverPos> jnt_to_pose_solver_; /**< Position forward kinematics solver */
- std::vector<Eigen::Matrix<double, 2, 3> > cart_constraints_; /**< Cartesian box constraints */
- int count_; /**< Tick counter */
-
- /**
- * \brief Initializes the sinusoids
- */
- void initSinusoids(double min_freq, double max_freq, double freq_randomness);
-
- /**
- * \brief Performs kinematic constraint checks
- * \return true if a constraint is violate...
[truncated message content] |
