Revision: 9851
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9851&view=rev
Author: meeussen
Date: 2009-01-21 02:36:21 +0000 (Wed, 21 Jan 2009)
Log Message:
-----------
rename endeffector controllers to cartesian controllers to be consistent
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp
Removed Paths:
-------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_pose_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_twist_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_wrench_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_pose_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_twist_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_wrench_controller.cpp
Modified: pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-01-21 02:32:05 UTC (rev 9850)
+++ pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-01-21 02:36:21 UTC (rev 9851)
@@ -5,10 +5,9 @@
gensrv()
rospack_add_library(robot_mechanism_controllers
src/endeffector_constraint_controller.cpp
- src/endeffector_pose_controller.cpp
- src/endeffector_twist_controller.cpp
-
- src/endeffector_wrench_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_position_smoothing_controller.cpp
Copied: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h (from rev 9800, pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_pose_controller.h)
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h 2009-01-21 02:36:21 UTC (rev 9851)
@@ -0,0 +1,108 @@
+/*
+ * 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_POSE_CONTEROLLER_H
+#define CARTESIAN_POSE_CONTEROLLER_H
+
+#include <vector>
+#include "kdl/chain.hpp"
+#include "kdl/frames.hpp"
+#include "ros/node.h"
+#include "std_msgs/PoseStamped.h"
+#include "mechanism_model/controller.h"
+#include "robot_mechanism_controllers/cartesian_twist_controller.h"
+
+namespace controller {
+
+class CartesianPoseController : public Controller
+{
+public:
+ CartesianPoseController();
+ ~CartesianPoseController();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ void update();
+
+ // input of the controller
+ KDL::Frame pose_desi_, pose_meas_;
+ KDL::Twist twist_ff_;
+
+
+private:
+ KDL::Frame getPose();
+
+ unsigned int num_joints_, num_segments_;
+ double last_time_;
+
+ // robot structure
+ mechanism::RobotState *robot_;
+
+ // pid controllers
+ std::vector<control_toolbox::Pid> pid_controller_;
+
+ // kdl stuff for kinematics
+ KDL::Chain chain_;
+ KDL::ChainFkSolverPos* jnt_to_pose_solver_;
+
+ // to get joint positions, velocities, and to set joint torques
+ std::vector<mechanism::JointState*> joints_;
+
+ // internal twist controller
+ CartesianTwistController twist_controller_;
+};
+
+
+
+
+class CartesianPoseControllerNode : public Controller
+{
+ public:
+ CartesianPoseControllerNode() {};
+ ~CartesianPoseControllerNode();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ void update();
+ void command();
+
+ private:
+ std::string topic_;
+
+ CartesianPoseController controller_;
+
+ std_msgs::PoseStamped pose_msg_;
+};
+
+} // namespace
+
+
+#endif
Copied: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller.h (from rev 9800, pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_twist_controller.h)
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller.h (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller.h 2009-01-21 02:36:21 UTC (rev 9851)
@@ -0,0 +1,118 @@
+/*
+ * 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_TWIST_CONTEROLLER_H
+#define CARTESIAN_TWIST_CONTEROLLER_H
+
+#include <vector>
+#include "kdl/chain.hpp"
+#include "kdl/frames.hpp"
+#include "ros/node.h"
+#include "robot_msgs/Twist.h"
+#include "mechanism_model/controller.h"
+#include "tf/transform_datatypes.h"
+#include "robot_mechanism_controllers/cartesian_wrench_controller.h"
+#include "joy/Joy.h"
+#include <control_toolbox/pid.h>
+
+namespace controller {
+
+class CartesianTwistController : public Controller
+{
+public:
+ CartesianTwistController();
+ ~CartesianTwistController();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ void update();
+
+ // input of the controller
+ KDL::Twist twist_desi_, twist_meas_;
+
+ // output of the controller
+ KDL::Wrench wrench_out_;
+
+
+private:
+ unsigned int num_joints_, num_segments_;
+ double last_time_;
+
+ // robot structure
+ mechanism::RobotState *robot_;
+
+ // pid controllers
+ std::vector<control_toolbox::Pid> pid_controller_;
+
+ // kdl stuff for kinematics
+ KDL::Chain chain_;
+ KDL::ChainFkSolverVel* jnt_to_twist_solver_;
+
+ // to get joint positions, velocities, and to set joint torques
+ std::vector<mechanism::JointState*> joints_;
+
+ // internal wrench controller
+ CartesianWrenchController wrench_controller_;
+};
+
+
+
+
+
+
+class CartesianTwistControllerNode : public Controller
+{
+ public:
+ CartesianTwistControllerNode() {};
+ ~CartesianTwistControllerNode();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ void update();
+ void command();
+
+ // callback functions for joystick
+ void joystick();
+
+ private:
+ double joystick_max_trans_, joystick_max_rot_;
+ std::string topic_;
+
+ CartesianTwistController controller_;
+
+ robot_msgs::Twist twist_msg_;
+ joy::Joy joystick_msg_;
+};
+
+} // namespace
+
+
+#endif
Copied: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h (from rev 9800, pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_wrench_controller.h)
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h 2009-01-21 02:36:21 UTC (rev 9851)
@@ -0,0 +1,99 @@
+/*
+ * 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_WRENCH_CONTEROLLER_H
+#define CARTESIAN_WRENCH_CONTEROLLER_H
+
+#include <vector>
+#include <kdl/chain.hpp>
+#include <kdl/frames.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
+#include "ros/node.h"
+#include "robot_msgs/Wrench.h"
+#include "mechanism_model/controller.h"
+#include "tf/transform_datatypes.h"
+#include "joy/Joy.h"
+
+namespace controller {
+
+class CartesianWrenchController : public Controller
+{
+public:
+ CartesianWrenchController();
+ ~CartesianWrenchController();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ void update();
+
+ // input of the controller
+ KDL::Wrench wrench_desi_;
+
+private:
+ unsigned int num_joints_, num_segments_;
+
+ // kdl stuff for kinematics
+ KDL::Chain chain_;
+ KDL::ChainJntToJacSolver* jnt_to_jac_solver_;
+
+ // to get joint positions, velocities, and to set joint torques
+ std::vector<mechanism::JointState*> joints_;
+
+};
+
+
+
+
+
+
+class CartesianWrenchControllerNode : public Controller
+{
+ public:
+ CartesianWrenchControllerNode() {};
+ ~CartesianWrenchControllerNode();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ void update();
+ void command();
+
+ private:
+ std::string topic_;
+
+ CartesianWrenchController controller_;
+
+ robot_msgs::Wrench wrench_msg_;
+};
+
+} // namespace
+
+
+#endif
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_pose_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_pose_controller.h 2009-01-21 02:32:05 UTC (rev 9850)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_pose_controller.h 2009-01-21 02:36:21 UTC (rev 9851)
@@ -1,116 +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 ENDEFFECTOR_POSE_CONTEROLLER_H
-#define ENDEFFECTOR_POSE_CONTEROLLER_H
-
-#include <vector>
-#include "kdl/chain.hpp"
-#include "kdl/frames.hpp"
-#include "ros/node.h"
-#include "std_msgs/PoseStamped.h"
-#include "mechanism_model/controller.h"
-#include "robot_mechanism_controllers/endeffector_twist_controller.h"
-
-namespace controller {
-
-class EndeffectorPoseController : public Controller
-{
-public:
- EndeffectorPoseController();
- ~EndeffectorPoseController();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- void update();
-
- // input of the controller
- KDL::Frame pose_desi_, pose_meas_;
-
- // output of the controller
- KDL::Twist twist_out_;
-
-
-private:
- KDL::Frame getPose();
-
- unsigned int num_joints_, num_segments_;
- double last_time_;
-
- // robot structure
- mechanism::RobotState *robot_;
-
- // pid controllers
- std::vector<control_toolbox::Pid> pid_controller_;
-
- // kdl stuff for kinematics
- KDL::Chain chain_;
- KDL::ChainFkSolverPos* jnt_to_pose_solver_;
-
- // to get joint positions, velocities, and to set joint torques
- std::vector<mechanism::JointState*> joints_;
-
- // internal twist controller
- EndeffectorTwistController twist_controller_;
-};
-
-
-
-
-class EndeffectorPoseControllerNode : public Controller
-{
- public:
- EndeffectorPoseControllerNode() {};
- ~EndeffectorPoseControllerNode();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- void update();
- void command();
-
- // callback functions for joystick
- void joystick();
-
- private:
- double joystick_max_trans_, joystick_max_rot_;
- std::string topic_;
-
- EndeffectorPoseController controller_;
-
- std_msgs::PoseStamped pose_msg_;
- joy::Joy joystick_msg_;
- KDL::Twist joystick_twist_;
-};
-
-} // namespace
-
-
-#endif
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_twist_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_twist_controller.h 2009-01-21 02:32:05 UTC (rev 9850)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_twist_controller.h 2009-01-21 02:36:21 UTC (rev 9851)
@@ -1,118 +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 ENDEFFECTOR_TWIST_CONTEROLLER_H
-#define ENDEFFECTOR_TWIST_CONTEROLLER_H
-
-#include <vector>
-#include "kdl/chain.hpp"
-#include "kdl/frames.hpp"
-#include "ros/node.h"
-#include "robot_msgs/Twist.h"
-#include "mechanism_model/controller.h"
-#include "tf/transform_datatypes.h"
-#include "robot_mechanism_controllers/endeffector_wrench_controller.h"
-#include "joy/Joy.h"
-#include <control_toolbox/pid.h>
-
-namespace controller {
-
-class EndeffectorTwistController : public Controller
-{
-public:
- EndeffectorTwistController();
- ~EndeffectorTwistController();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- void update();
-
- // input of the controller
- KDL::Twist twist_desi_, twist_meas_;
-
- // output of the controller
- KDL::Wrench wrench_out_;
-
-
-private:
- unsigned int num_joints_, num_segments_;
- double last_time_;
-
- // robot structure
- mechanism::RobotState *robot_;
-
- // pid controllers
- std::vector<control_toolbox::Pid> pid_controller_;
-
- // kdl stuff for kinematics
- KDL::Chain chain_;
- KDL::ChainFkSolverVel* jnt_to_twist_solver_;
-
- // to get joint positions, velocities, and to set joint torques
- std::vector<mechanism::JointState*> joints_;
-
- // internal wrench controller
- EndeffectorWrenchController wrench_controller_;
-};
-
-
-
-
-
-
-class EndeffectorTwistControllerNode : public Controller
-{
- public:
- EndeffectorTwistControllerNode() {};
- ~EndeffectorTwistControllerNode();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- void update();
- void command();
-
- // callback functions for joystick
- void joystick();
-
- private:
- double joystick_max_trans_, joystick_max_rot_;
- std::string topic_;
-
- EndeffectorTwistController controller_;
-
- robot_msgs::Twist twist_msg_;
- joy::Joy joystick_msg_;
-};
-
-} // namespace
-
-
-#endif
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_wrench_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_wrench_controller.h 2009-01-21 02:32:05 UTC (rev 9850)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_wrench_controller.h 2009-01-21 02:36:21 UTC (rev 9851)
@@ -1,99 +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 ENDEFFECTOR_WRENCH_CONTEROLLER_H
-#define ENDEFFECTOR_WRENCH_CONTEROLLER_H
-
-#include <vector>
-#include <kdl/chain.hpp>
-#include <kdl/frames.hpp>
-#include <kdl/chainjnttojacsolver.hpp>
-#include "ros/node.h"
-#include "robot_msgs/Wrench.h"
-#include "mechanism_model/controller.h"
-#include "tf/transform_datatypes.h"
-#include "joy/Joy.h"
-
-namespace controller {
-
-class EndeffectorWrenchController : public Controller
-{
-public:
- EndeffectorWrenchController();
- ~EndeffectorWrenchController();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- void update();
-
- // input of the controller
- KDL::Wrench wrench_desi_;
-
-private:
- unsigned int num_joints_, num_segments_;
-
- // kdl stuff for kinematics
- KDL::Chain chain_;
- KDL::ChainJntToJacSolver* jnt_to_jac_solver_;
-
- // to get joint positions, velocities, and to set joint torques
- std::vector<mechanism::JointState*> joints_;
-
-};
-
-
-
-
-
-
-class EndeffectorWrenchControllerNode : public Controller
-{
- public:
- EndeffectorWrenchControllerNode() {};
- ~EndeffectorWrenchControllerNode();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- void update();
- void command();
-
- private:
- std::string topic_;
-
- EndeffectorWrenchController controller_;
-
- robot_msgs::Wrench wrench_msg_;
-};
-
-} // namespace
-
-
-#endif
Copied: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp (from rev 9800, pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_pose_controller.cpp)
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp 2009-01-21 02:36:21 UTC (rev 9851)
@@ -0,0 +1,266 @@
+/*
+ * 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 "urdf/parser.h"
+#include <algorithm>
+#include "robot_kinematics/robot_kinematics.h"
+#include "robot_mechanism_controllers/cartesian_pose_controller.h"
+
+
+using namespace KDL;
+namespace controller {
+
+
+
+ROS_REGISTER_CONTROLLER(CartesianPoseController)
+
+
+CartesianPoseController::CartesianPoseController()
+: jnt_to_pose_solver_(NULL),
+ joints_(0,(mechanism::JointState*)NULL)
+{}
+
+CartesianPoseController::~CartesianPoseController()
+{
+ if (jnt_to_pose_solver_) delete jnt_to_pose_solver_;
+}
+
+
+
+bool CartesianPoseController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ fprintf(stderr, "initializing pose controller\n");
+
+ // test if we got robot pointer
+ assert(robot);
+ robot_ = robot;
+
+ // get pid controller
+ TiXmlElement *p_pose = config->FirstChildElement("pid_pose");
+ control_toolbox::Pid pid_pose;
+ pid_pose.initXml(p_pose);
+ for (unsigned int i=0; i<6; i++)
+ pid_controller_.push_back(pid_pose);
+ fprintf(stderr, "pid controllers created\n");
+
+ // time
+ last_time_ = robot->hw_->current_time_;
+
+ // create twist controller
+ twist_controller_.initXml(robot, config);
+
+ // parse robot description from xml file
+ ros::Node *node = ros::Node::instance();
+ robot_kinematics::RobotKinematics robot_kinematics ;
+ string robot_desc;
+ node->param("robotdesc/pr2", robot_desc, string("")) ;
+ printf("RobotDesc.length() = %u\n", robot_desc.length());
+ robot_kinematics.loadString(robot_desc.c_str()) ;
+ robot_kinematics::SerialChain* serial_chain = robot_kinematics.getSerialChain("right_arm");
+ if (serial_chain == NULL)
+ fprintf(stderr, "Got NULL Chain\n") ;
+
+ // convert description to KDL chain
+ chain_ = serial_chain->chain;
+ num_joints_ = chain_.getNrOfJoints();
+ num_segments_ = chain_.getNrOfSegments();
+ printf("Extracted KDL Chain with %u Joints and %u segments\n", num_joints_, num_segments_ );
+ jnt_to_pose_solver_ = new ChainFkSolverPos_recursive(chain_);
+
+ // get chain
+ TiXmlElement *chain = config->FirstChildElement("chain");
+ if (!chain) {
+ fprintf(stderr, "Error: CartesianPoseController was not given a chain\n");
+ return false;
+ }
+
+ // get names for root and tip of robot
+ const char *root_name = chain->Attribute("root");
+ const char *tip_name = chain->Attribute("tip");
+ if (!root_name) {
+ fprintf(stderr, "Error: Chain element for CartesianPoseController must specify the root\n");
+ return false;
+ }
+ if (!tip_name) {
+ fprintf(stderr, "Error: Chain element for CartesianPoseController must specify the tip\n");
+ return false;
+ }
+
+ // test if we can get root from robot
+ if (!robot->getLinkState(root_name)) {
+ fprintf(stderr, "Error: link \"%s\" does not exist (CartesianPoseController)\n", root_name);
+ return false;
+ }
+
+ // get tip from robot
+ mechanism::LinkState *current = robot->getLinkState(tip_name);
+ if (!current) {
+ fprintf(stderr, "Error: link \"%s\" does not exist (CartesianPoseController)\n", tip_name);
+ return false;
+
+ }
+
+ // Works up the chain, from the tip to the root, and get joints
+ while (current->link_->name_ != std::string(root_name))
+ {
+ // get joint from current link
+ joints_.push_back(robot->getJointState(current->link_->joint_name_));
+ assert(joints_[joints_.size()-1]);
+
+ // get parent link
+ current = robot->getLinkState(current->link_->parent_name_);
+
+ if (!current) {
+ fprintf(stderr, "Error: for CartesianPoseController, tip is not connected to root\n");
+ return false;
+ }
+ }
+ // reverse order of joint vector
+ std::reverse(joints_.begin(), joints_.end());
+
+ // set desired pose to current pose
+ pose_desi_ = getPose();
+
+ // set the twist feedworward to zero
+ twist_ff_ = Twist::Zero();
+
+ return true;
+}
+
+
+
+
+
+
+void CartesianPoseController::update()
+{
+ // get current time
+ double time = robot_->hw_->current_time_;
+ double dt = time - last_time_;
+
+ // get current pose
+ pose_meas_ = getPose();
+
+ // pose feedback into twist
+ Twist twist_error = diff(pose_desi_, pose_meas_);
+ Twist twist_fb;
+ for (unsigned int i=0; i<6; i++)
+ twist_fb(i) = pid_controller_[i].updatePid(twist_error(i), dt);
+
+ // send feedback twist and feedforward twist to twist controller
+ twist_controller_.twist_desi_ = twist_fb + twist_ff_;
+ twist_controller_.update();
+
+ // remember time
+ last_time_ = time;
+}
+
+
+
+Frame CartesianPoseController::getPose()
+{
+ // check if joints are calibrated
+ for (unsigned int i = 0; i < joints_.size(); ++i) {
+ if (!joints_[i]->calibrated_)
+ fprintf(stderr,"Joint not calibrated\n");
+ }
+
+ // get the joint positions
+ JntArray jnt_pos(num_joints_);
+ for (unsigned int i=0; i<num_joints_; i++)
+ jnt_pos(i) = joints_[i]->position_;
+
+ // get cartesian pose
+ Frame result;
+ jnt_to_pose_solver_->JntToCart(jnt_pos, result);
+
+ return result;
+}
+
+
+
+
+ROS_REGISTER_CONTROLLER(CartesianPoseControllerNode)
+
+CartesianPoseControllerNode::~CartesianPoseControllerNode()
+{
+ ros::Node *node = ros::Node::instance();
+
+ node->unsubscribe(topic_ + "/command");
+}
+
+
+bool CartesianPoseControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ // get name of topic to listen to
+ ros::Node *node = ros::Node::instance();
+ topic_ = config->Attribute("topic") ? config->Attribute("topic") : "";
+ if (topic_ == "") {
+ fprintf(stderr, "No topic given to CartesianPoseControllerNode\n");
+ return false;
+ }
+
+ // initialize controller
+ if (!controller_.initXml(robot, config))
+ return false;
+
+ // subscribe to pose commands
+ node->subscribe(topic_ + "/command", pose_msg_,
+ &CartesianPoseControllerNode::command, this, 1);
+
+ return true;
+}
+
+
+void CartesianPoseControllerNode::update()
+{
+ controller_.update();
+}
+
+
+void CartesianPoseControllerNode::command()
+{
+ // convert to pose command
+ controller_.pose_desi_.p(0) = pose_msg_.pose.position.x;
+ controller_.pose_desi_.p(1) = pose_msg_.pose.position.y;
+ controller_.pose_desi_.p(2) = pose_msg_.pose.position.z;
+ controller_.pose_desi_.M = Rotation::Quaternion(pose_msg_.pose.orientation.x, pose_msg_.pose.orientation.y,
+ pose_msg_.pose.orientation.z, pose_msg_.pose.orientation.w);
+}
+
+
+
+}; // namespace
+
Copied: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp (from rev 9800, pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_twist_controller.cpp)
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp 2009-01-21 02:36:21 UTC (rev 9851)
@@ -0,0 +1,286 @@
+/*
+ * 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 "urdf/parser.h"
+#include <algorithm>
+#include "robot_kinematics/robot_kinematics.h"
+#include "robot_mechanism_controllers/cartesian_twist_controller.h"
+
+
+using namespace KDL;
+namespace controller {
+
+
+ROS_REGISTER_CONTROLLER(CartesianTwistController)
+
+
+CartesianTwistController::CartesianTwistController()
+: jnt_to_twist_solver_(NULL),
+ joints_(0,(mechanism::JointState*)NULL)
+{}
+
+
+
+CartesianTwistController::~CartesianTwistController()
+{
+ if (jnt_to_twist_solver_) delete jnt_to_twist_solver_;
+}
+
+
+
+bool CartesianTwistController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ fprintf(stderr, "initializing twist controller\n");
+
+ // test if we got robot pointer
+ assert(robot);
+ robot_ = robot;
+
+ // get pid controller
+ TiXmlElement *p_trans = config->FirstChildElement("pid_trans");
+ control_toolbox::Pid pid_trans;
+ pid_trans.initXml(p_trans);
+ for (unsigned int i=0; i<3; i++)
+ pid_controller_.push_back(pid_trans);
+
+ TiXmlElement *p_rot = config->FirstChildElement("pid_rot");
+ control_toolbox::Pid pid_rot;
+ pid_rot.initXml(p_rot);
+ for (unsigned int i=0; i<3; i++)
+ pid_controller_.push_back(pid_rot);
+ fprintf(stderr, "pid controllers created\n");
+
+
+ // time
+ last_time_ = robot->hw_->current_time_;
+
+ // set disired twist to 0
+ twist_desi_ = Twist::Zero();
+
+ // create wrench controller
+ wrench_controller_.initXml(robot, config);
+
+ // parse robot description from xml file
+ ros::Node *node = ros::Node::instance();
+ robot_kinematics::RobotKinematics robot_kinematics ;
+ string robot_desc;
+ node->param("robotdesc/pr2", robot_desc, string("")) ;
+ printf("RobotDesc.length() = %u\n", robot_desc.length());
+ robot_kinematics.loadString(robot_desc.c_str()) ;
+ robot_kinematics::SerialChain* serial_chain = robot_kinematics.getSerialChain("right_arm");
+ if (serial_chain == NULL)
+ fprintf(stderr, "Got NULL Chain\n") ;
+
+ // convert description to KDL chain
+ chain_ = serial_chain->chain;
+ num_joints_ = chain_.getNrOfJoints();
+ num_segments_ = chain_.getNrOfSegments();
+ printf("Extracted KDL Chain with %u Joints and %u segments\n", num_joints_, num_segments_ );
+ jnt_to_twist_solver_ = new ChainFkSolverVel_recursive(chain_);
+
+ // get chain
+ TiXmlElement *chain = config->FirstChildElement("chain");
+ if (!chain) {
+ fprintf(stderr, "Error: CartesianTwistController was not given a chain\n");
+ return false;
+ }
+
+ // get names for root and tip of robot
+ const char *root_name = chain->Attribute("root");
+ const char *tip_name = chain->Attribute("tip");
+ if (!root_name) {
+ fprintf(stderr, "Error: Chain element for CartesianTwistController must specify the root\n");
+ return false;
+ }
+ if (!tip_name) {
+ fprintf(stderr, "Error: Chain element for CartesianTwistController must specify the tip\n");
+ return false;
+ }
+
+ // test if we can get root from robot
+ if (!robot->getLinkState(root_name)) {
+ fprintf(stderr, "Error: link \"%s\" does not exist (CartesianTwistController)\n", root_name);
+ return false;
+ }
+
+ // get tip from robot
+ mechanism::LinkState *current = robot->getLinkState(tip_name);
+ if (!current) {
+ fprintf(stderr, "Error: link \"%s\" does not exist (CartesianTwistController)\n", tip_name);
+ return false;
+
+ }
+
+ // Works up the chain, from the tip to the root, and get joints
+ while (current->link_->name_ != std::string(root_name))
+ {
+ // get joint from current link
+ joints_.push_back(robot->getJointState(current->link_->joint_name_));
+ assert(joints_[joints_.size()-1]);
+
+ // get parent link
+ current = robot->getLinkState(current->link_->parent_name_);
+
+ if (!current) {
+ fprintf(stderr, "Error: for CartesianTwistController, tip is not connected to root\n");
+ return false;
+ }
+ }
+ // reverse order of joint vector
+ std::reverse(joints_.begin(), joints_.end());
+
+
+ fprintf(stderr, "initialized twist controller\n");
+
+ return true;
+}
+
+
+
+
+
+
+void CartesianTwistController::update()
+{
+ // get current time
+ double time = robot_->hw_->current_time_;
+
+ // check if joints are calibrated
+ for (unsigned int i = 0; i < joints_.size(); ++i) {
+ if (!joints_[i]->calibrated_)
+ return;
+ }
+
+ // get the joint positions and velocities
+ JntArrayVel jnt_posvel(num_joints_);
+ for (unsigned int i=0; i<num_joints_; i++){
+ jnt_posvel.q(i) = joints_[i]->position_;
+ jnt_posvel.qdot(i) = joints_[i]->velocity_;
+ }
+
+ // get cartesian twist error
+ FrameVel twist;
+ jnt_to_twist_solver_->JntToCart(jnt_posvel, twist);
+ twist_meas_ = twist.deriv();
+ Twist error = twist_meas_ - twist_desi_;
+ double dt = time - last_time_;
+
+ // pid feedback
+ for (unsigned int i=0; i<6; i++)
+ wrench_out_(i) = pid_controller_[i].updatePid(error(i), dt);
+
+ // send wrench to wrench controller
+ wrench_controller_.wrench_desi_ = wrench_out_;
+ wrench_controller_.update();
+
+ // remember time
+ last_time_ = time;
+}
+
+
+
+
+
+
+
+
+ROS_REGISTER_CONTROLLER(CartesianTwistControllerNode)
+
+CartesianTwistControllerNode::~CartesianTwistControllerNode()
+{
+ ros::Node *node = ros::Node::instance();
+
+ node->unsubscribe(topic_ + "/command");
+ node->unsubscribe("spacenav/joy");
+}
+
+
+bool CartesianTwistControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ // get name of topic_ to listen to
+ ros::Node *node = ros::Node::instance();
+ topic_ = config->Attribute("topic") ? config->Attribute("topic") : "";
+ if (topic_ == "") {
+ fprintf(stderr, "No topic given to CartesianTwistControllerNode\n");
+ return false;
+ }
+
+ // get parameters
+ node->param("arm_twist/joystick_max_trans", joystick_max_trans_, 0.0);
+ node->param("arm_twist/joystick_max_rot", joystick_max_rot_, 0.0);
+
+ // initialize controller
+ if (!controller_.initXml(robot, config))
+ return false;
+
+ // subscribe to twist commands
+ node->subscribe(topic_ + "/command", twist_msg_,
+ &CartesianTwistControllerNode::command, this, 1);
+
+ // subscribe to joystick commands
+ node->subscribe("spacenav/joy", joystick_msg_,
+ &CartesianTwistControllerNode::joystick, this, 1);
+
+ return true;
+}
+
+
+void CartesianTwistControllerNode::update()
+{
+ controller_.update();
+}
+
+
+void CartesianTwistControllerNode::command()
+{
+ // convert to twist command
+ controller_.twist_desi_.vel(0) = twist_msg_.vel.x;
+ controller_.twist_desi_.vel(1) = twist_msg_.vel.y;
+ controller_.twist_desi_.vel(2) = twist_msg_.vel.z;
+ controller_.twist_desi_.rot(0) = twist_msg_.rot.x;
+ controller_.twist_desi_.rot(1) = twist_msg_.rot.y;
+ controller_.twist_desi_.rot(2) = twist_msg_.rot.z;
+}
+
+
+void CartesianTwistControllerNode::joystick()
+{
+ // convert to twist command
+ for (unsigned int i=0; i<3; i++){
+ controller_.twist_desi_.vel(i) = joystick_msg_.axes[i] * joystick_max_trans_;
+ controller_.twist_desi_.rot(i) = joystick_msg_.axes[i+3] * joystick_max_rot_;
+ }
+}
+
+
+}; // namespace
Copied: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp (from rev 9800, pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_wrench_controller.cpp)
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp 2009-01-21 02:36:21 UTC (rev 9851)
@@ -0,0 +1,234 @@
+/*
+ * 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 "urdf/parser.h"
+#include <algorithm>
+#include "robot_kinematics/robot_kinematics.h"
+#include "robot_mechanism_controllers/cartesian_wrench_controller.h"
+
+
+static const double JOYSTICK_MAX_FORCE = 20.0;
+static const double JOYSTICK_MAX_TORQUE = 0.75;
+
+
+using namespace KDL;
+
+namespace controller {
+
+ROS_REGISTER_CONTROLLER(CartesianWrenchController)
+
+CartesianWrenchController::CartesianWrenchController()
+: jnt_to_jac_solver_(NULL),
+ joints_(0,(mechanism::JointState*)NULL)
+{
+ printf("CartesianWrenchController constructor\n");
+}
+
+
+
+CartesianWrenchController::~CartesianWrenchController()
+{
+ if (jnt_to_jac_solver_) delete jnt_to_jac_solver_;
+}
+
+
+
+bool CartesianWrenchController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ // set disired wrench to 0
+ for (unsigned int i=0; i<3; i++){
+ wrench_desi_.force(i) = 0;
+ wrench_desi_.torque(i) = 0;
+ }
+
+ // parse robot description from xml file
+ ros::Node *node = ros::Node::instance();
+ robot_kinematics::RobotKinematics robot_kinematics ;
+ string robot_desc;
+ node->param("robotdesc/pr2", robot_desc, string("")) ;
+ robot_kinematics.loadString(robot_desc.c_str()) ;
+ robot_kinematics::SerialChain* serial_chain = robot_kinematics.getSerialChain("right_arm");
+ if (serial_chain == NULL)
+ fprintf(stderr, "Got NULL Chain\n") ;
+
+ // convert description to KDL chain
+ chain_ = serial_chain->chain;
+ num_joints_ = chain_.getNrOfJoints();
+ num_segments_ = chain_.getNrOfSegments();
+ printf("Extracted KDL Chain with %u Joints and %u segments\n", num_joints_, num_segments_ );
+ jnt_to_jac_solver_ = new ChainJntToJacSolver(chain_);
+
+ // get chain
+ TiXmlElement *chain = config->FirstChildElement("chain");
+ if (!chain) {
+ fprintf(stderr, "Error: CartesianWrenchController was not given a chain\n");
+ return false;
+ }
+
+ // get names for root and tip of robot
+ const char *root_name = chain->Attribute("root");
+ const char *tip_name = chain->Attribute("tip");
+ if (!root_name) {
+ fprintf(stderr, "Error: Chain element for CartesianWrenchController must specify the root\n");
+ return false;
+ }
+ if (!tip_name) {
+ fprintf(stderr, "Error: Chain element for CartesianWrenchController must specify the tip\n");
+ return false;
+ }
+
+ // test if we can get root from robot
+ assert(robot);
+ if (!robot->getLinkState(root_name)) {
+ fprintf(stderr, "Error: link \"%s\" does not exist (CartesianWrenchController)\n", root_name);
+ return false;
+ }
+
+ // get tip from robot
+ mechanism::LinkState *current = robot->getLinkState(tip_name);
+ if (!current) {
+ fprintf(stderr, "Error: link \"%s\" does not exist (CartesianWrenchController)\n", tip_name);
+ return false;
+ }
+
+ // Works up the chain, from the tip to the root, and get joints
+ while (current->link_->name_ != std::string(root_name))
+ {
+ // get joint from current link
+ joints_.push_back(robot->getJointState(current->link_->joint_name_));
+ assert(joints_[joints_.size()-1]);
+
+ // get parent link
+ current = robot->getLinkState(current->link_->parent_name_);
+
+ if (!current) {
+ fprintf(stderr, "Error: for CartesianWrenchController, tip is not connected to root\n");
+ return false;
+ }
+ }
+ // reverse order of joint vector
+ std::reverse(joints_.begin(), joints_.end());
+
+ // get control parameters
+
+
+
+ return true;
+}
+
+
+
+
+void CartesianWrenchController::update()
+{
+ // check if joints are calibrated
+ for (unsigned int i = 0; i < joints_.size(); ++i) {
+ if (!joints_[i]->calibrated_)
+ return;
+ }
+
+ // get the joint positions
+ JntArray jnt_pos(num_joints_);
+ for (unsigned int i=0; i<num_joints_; i++)
+ jnt_pos(i) = joints_[i]->position_;
+
+ // get the chain jacobian
+ Jacobian jacobian(num_joints_, num_segments_);
+ jnt_to_jac_solver_->JntToJac(jnt_pos, jacobian);
+
+ // convert the wrench into joint torques
+ JntArray jnt_torq(num_joints_);
+ for (unsigned int i=0; i<num_joints_; i++){
+ jnt_torq(i) = 0;
+ for (unsigned int j=0; j<6; j++)
+ jnt_torq(i) += (jacobian(j,i) * wrench_desi_(j));
+ joints_[i]->commanded_effort_ = jnt_torq(i);
+ }
+}
+
+
+
+
+
+
+
+
+
+ROS_REGISTER_CONTROLLER(CartesianWrenchControllerNode)
+
+CartesianWrenchControllerNode::~CartesianWrenchControllerNode()
+{
+ ros::Node *node = ros::Node::instance();
+ node->unsubscribe(topic_ + "/command");
+}
+
+bool CartesianWrenchControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ // get name of topic to listen to from xml file
+ ros::Node *node = ros::Node::instance();
+ topic_ = config->Attribute("topic") ? config->Attribute("topic") : "";
+ if (topic_ == "") {
+ fprintf(stderr, "No topic given to CartesianWrenchControllerNode\n");
+ return false;
+ }
+
+ // initialize controller
+ if (!controller_.initXml(robot, config))
+ return false;
+
+ // subscribe to wrench commands
+ node->subscribe(topic_ + "/command", wrench_msg_,
+ &CartesianWrenchControllerNode::command, this, 1);
+
+ return true;
+}
+
+
+void CartesianWrenchControllerNode::update()
+{
+ controller_.update();
+}
+
+
+void CartesianWrenchControllerNode::command()
+{
+ // convert to wrench command
+ controller_.wrench_desi_.force(0) = wrench_msg_.force.x;
+ controller_.wrench_desi_.force(1) = wrench_msg_.force.y;
+ controller_.wrench_desi_.force(2) = wrench_msg_.force.z;
+ controller_.wrench_desi_.torque(0) = wrench_msg_.torque.x;
+ controller_.wrench_desi_.torque(1) = wrench_msg_.torque.y;
+ controller_.wrench_desi_.torque(2) = wrench_msg_.torque.z;
+}
+
+}; // namespace
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_pose_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_pose_controller.cpp 2009-01-21 02:32:05 UTC (rev 9850)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_pose_controller.cpp 2009-01-21 02:36:21 UTC (rev 9851)
@@ -1,284 +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 "urdf/parser.h"
-#include <algorithm>
-#include "robot_kinematics/robot_kinematics.h"
-#include "robot_mechanism_controllers/endeffector_pose_controller.h"
-
-
-using namespace KDL;
-namespace controller {
-
-
-
-ROS_REGISTER_CONTROLLER(EndeffectorPoseController)
-
-
-EndeffectorPoseController::EndeffectorPoseController()
-: jnt_to_pose_solver_(NULL),
- joints_(0,(mechanism::JointState*)NULL)
-{}
-
-EndeffectorPoseController::~EndeffectorPoseController()
-{
- if (jnt_to_pose_solver_) delete jnt_to_pose_solver_;
-}
-
-
-
-bool EndeffectorPoseController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- fprintf(stderr, "initializing pose controller\n");
-
- // test if we got robot pointer
- assert(robot);
- robot_ = robot;
-
- // get pid controller
- TiXmlElement *p_pose = config->FirstChildElement("pid_pose");
- control_toolbox::Pid pid_pose;
- pid_pose.initXml(p_pose);
- for (unsigned int i=0; i<6; i++)
- pid_controller_.push_back(pid_pose);
- fprintf(stderr, "pid controllers created\n");
-
- // time
- last_time_ = robot->hw_->current_time_;
-
- // create twist controller
- twist_controller_.initXml(robot, config);
-
- // parse robot description from xml file
- ros::Node *node = ros::Node::instance();
- robot_kinematics::RobotKinematics robot_kinematics ;
- string robot_desc;
- node->param("robotdesc/pr2", robot_desc, string("")) ;
- printf("RobotDesc.length() = %u\n", robot_desc.length());
- robot_kinematics.loadString(robot_desc.c_str()) ;
- robot_kinematics::SerialChain* serial_chain = robot_kinematics.getSerialChain("right_arm");
- if (serial_chain == NULL)
- fprintf(stderr, "Got NULL Chain\n") ;
-
- // convert description to KDL chain
- chain_ = serial_chain->chain;
- num_joints_ = chain_.getNrOfJoints();
- num_segments_ = chain_.getNrOfSegments();
- printf("Extracted KDL Chain with %u Joints and %u segments\n", num_joints_, num_segments_ );
- jnt_to_pose_solver_ = new ChainFkSolverPos_recursive(chain_);
-
- // get chain
- TiXmlElement *chain = config->FirstChildElement("chain");
- if (!chain) {
- fprintf(stderr, "Error: EndeffectorPoseController was not given a chain\n");
- return false;
- }
-
- // get names for root and tip of robot
- const char *root_name = chain->Attribute("root");
- const char *tip_name = chain->Attribute("tip");
- if (!root_name) {
- fprintf(stderr, "Error: Chain element for EndeffectorPoseController must specify the root\n");
- return false;
- }
- if (!tip_name) {
- fprintf(stderr, "Error: Chain element for EndeffectorPoseController must specify the tip\n");
- return false;
- }
-
- // test if we can get root from robot
- if (!robot->getLinkState(root_name)) {
- fprintf(stderr, "Error: link \"%s\" does not exist (EndeffectorPoseController)\n", root_name);
- return false;
- }
-
- // get tip from robot
- mechanism::LinkState *current = robot->getLinkState(tip_name);
- if (!current) {
- fprintf(stderr, "Error: link \"%s\" does not exist (EndeffectorPoseController)\n", tip_name);
- return false;
-
- }
-
- // Works up the chain, from the tip to the root, and get joints
- while (current->link_->name_ != std::string(root_name))
- {
- // get joint from current link
- joints_.push_back(robot->getJointState(current->link_->joint_name_));
- assert(joints_[joints_.size()-1]);
-
- // get parent link
- current = robot->getLinkState(current->link_->parent_name_);
-
- if (!current) {
- fprintf(stderr, "Error: for EndeffectorPoseController, tip is not connected to root\n");
- return false;
- }
- }
- // reverse order of joint vector
- std::reverse(joints_.begin(), joints_.end());
-
- // set desired pose to current pose
- pose_desi_ = getPose();
-
- return true;
-}
-
-
-
-
-
-
-void EndeffectorPoseController::update()
-{
- // get current time
- double time = robot_->hw_->current_time_;
- double dt = time - last_time_;
-
- // get current pose
- pose_meas_ = getPose();
-
- // pose feedback into twist
- Twist twist_error = diff(pose_desi_, pose_meas_);
- for (unsigned int i=0; i<6; i++)
- twist_out_(i) = pid_controller_[i].updatePid(twist_error(i), dt);
-
- // send twist to twist controller
- twist_controller_.twist_desi_ = twist_out_;
- twist_controller_.update();
-
- // remember time
- last_time_ = time;
-}
-
-
-
-Frame EndeffectorPoseController::getPose()
-{
- // check if joints are calibrated
- for (unsigned int i = 0; i < joints_.size(); ++i) {
- if (!joints_[i]->calibrated_)
- fprintf(stderr,"Joint not calibrated\n");
- }
-
- // get the joint positions
- JntArray jnt_pos(num_joints_);
- for (unsigned int i=0; i<num_joints_; i++)
- jnt_pos(i) = joints_[i]->position_;
-
- // get endeffector pose
- Frame result;
- jnt_to_pose_solver_->JntToCart(jnt_pos, result);
-
- return result;
-}
-
-
-
-
-ROS_REGISTER_CONTROLLER(EndeffectorPoseControllerNode)
-
-EndeffectorPoseControllerNode::~EndeffectorPoseControllerNode()
-{
- ros::Node *node = ros::Node::instance();
-
- node->unsubscribe(topic_ + "/command");
- node->unsubscribe("spacenav/joy");
-}
-
-
-bool EndeffectorPoseControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- // get name of topic to listen to
- ros::Node *node = ros::Node::instance();
- topic_ = config->Attribute("topic") ? config->Attribute("topic") : "";
- if (topic_ == "") {
- fprintf(stderr, "No topic given to EndeffectorPoseControllerNode\n");
- return false;
- }
-
- // get parameters
- node->param("arm_pose/joystick_max_trans", joystick_max_trans_, 0.0);
- node->param("arm_pose/joystick_max_rot", joystick_max_rot_, 0.0);
-
- // initialize controller
- if (!controller_.initXml(robot, config))
- return false;
-
- // subscribe to pose commands
- node->subscribe(topic_ + "/command", pose_msg_,
- &EndeffectorPoseControllerNode::command, this, 1);
-
- // subscribe to joystick commands
- node->subscribe("spacenav/joy", joystick_msg_,
- &EndeffectorPoseControllerNode::joystick, this, 1);
-
- joystick_twist_ = Twist::Zero();
- return true;
-}
-
-
-void EndeffectorPoseControllerNode::update()
-{
- controller_.update();
-}
-
-
-void EndeffectorPoseControllerNode::command()
-{
- // convert to pose command
- controller_.pose_desi_.p(0) = pose_msg_.pose.position.x;
- controller_.pose_desi_.p(1) = pose_msg_.pose.position.y;
- controller_.pose_desi_.p(2) = pose_msg_.pose.position.z;
- controller_.pose_desi_.M = Rotation::Quaternion(pose_msg_.pose.orientation.x, pose_msg_.pose.orientation.y,
- pose_msg_.pose.orientation.z, pose_msg_.pose.orientation.w);
-}
-
-
-
-void EndeffectorPoseControllerNode::joystick()
-{
- // convert to pose command
- for (unsigned int i=0; i<3; i++){
- joystick_twist_.vel(i) = joystick_msg_.axes[i] * joystick_max_trans_;
- joystick_twist_.rot(i) = joystick_msg_.axes[i+3] * joystick_max_rot_;
- }
-
- controller_.pose_desi_ = addDelta(controller_.pose_desi_, joystick_twist_);
-}
-
-
-}; // namespace
-
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_twist_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_twist_controller.cpp 2009-01-21 02:32:05 UTC (rev 9850)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_twist_controller.cpp 2009-01-21 02:36:21 UTC (rev 9851)
@@ -1,286 +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 "urdf/parser.h"
-#include <algorithm>
-#include "robot_kinematics/robot_kinematics.h"
-#include "robot_mechanism_controllers/endeffector_twist_controller.h"
-
-
-using namespace KDL;
-namespace controller {
-
-
-ROS_REGISTER_CONTROLLER(EndeffectorTwistController)
-
-
-EndeffectorTwistController::EndeffectorTwistController()
-: jnt_to_twist_solver_(NULL),
- joints_(0,(mechanism::JointState*)NULL)
-{}
-
-
-
-EndeffectorTwistController::~EndeffectorTwistController()
-{
- if (jnt_to_twist_solver_) delete jnt_to_twist_solver_;
-}
-
-
-
-bool EndeffectorTwistController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- fprintf(stderr, "initializing twist controller\n");
-
- // test if we got robot pointer
- assert(robot);
- robot_ = robot;
-
- // get pid controller
- TiXmlElement *p_trans = config->FirstChildElement("pid_trans");
- control_toolbox::Pid pid_trans;
- pid_trans.initXml(p_trans);
- for (unsigned int i=0; i<3; i++)
- pid_controller_.push_back(pid_trans);
-
- TiXmlElement *p_rot = config->FirstChildElement("pid_rot");
- control_toolbox::Pid pid_rot;
- pid_rot.initXml(p_rot);
- for (unsigned int i=0; i<3; i++)
- pid_controller_.push_back(pid_rot);
- fprintf(stderr, "pid controllers created\n");
-
-
- // time
- last_time_ = robot->hw_->current_time_;
-
- // set disired twist to 0
- twist_desi_ = Twist::Zero();
-
- // create wrench controller
- wrench_controller_.initXml(robot, config);
-
- // parse robot description from xml file
- ros::Node *node = ros::Node::instance();
- robot_kinematics::RobotKinematics robot_kinematics ;
- string robot_desc;
- node->param("robotdesc/pr2", robot_desc, string("")) ;
- printf("RobotDesc.length() = %u\n", robot_desc.length());
- robot_kinematics.loadString(robot_desc.c_str()) ;
- robot_kinematics::SerialChain* serial_chain = robot_kinematics.getSerialChain("right_arm");
- if (serial_chain == NULL)
- fprintf(stderr, "Got NULL Chain\n") ;
-
- // convert description to KDL chain
- chain_ = serial_chain->chain;
- num_joints_ = chain_.getNrOfJoints();
- num_segments_ = chain_.getNrOfSegments();
- printf("Extracted KDL Chain with %u Joints and %u segments\n", num_joints_, num_segments_ );
- jnt_to_twist_solver_ = new ChainFkSolverVel_recursive(chain_);
-
- // get chain
- TiXmlElement *chain = config->FirstChildElement("chain");
- if (!chain) {
- fprintf(stderr, "Error: EndeffectorTwistController was not given a chain\n");
- return false;
- }
-
- // get names for root and tip of robot
- const char *root_name = chain->Attribute("root");
- const char *tip_name = chain->Attribute("tip");
- if (!root_name) {
- fprintf(stderr, "Error: Chain element for EndeffectorTwistController must specify the root\n");
- return false;
- }
- if (!tip_name) {
- fprintf(stderr, "Error: Chain element for EndeffectorTwistController must specify the tip\n");
- return false;
- }
-
- // test if we can get root from robot
- if (!robot->getLinkState(root_name)) {
- fprintf(stderr, "Error: link \"%s\" does not exist (EndeffectorTwistController)\n", root_name);
- return false;
- }
-
- // get tip from robot
- mechanism::LinkState *current = robot->getLinkState(tip_name);
- if (!current) {
- fprintf(stderr, "Error: link \"%s\" does not exist (EndeffectorTwistController)\n", tip_name);
- return false;
-
- }
-
- // Works up the chain, from the tip to the root, and get joints
- while (current->link_->name_ != std::string(root_name))
- {
- // get joint from current link
- joints_.push_back(robot->getJointState(current->link_->joint_name_));
- assert(joints_[joints_.size()-1]);
-
- // get parent link
- current = robot->getLinkState(current->link_->parent_name_);
-
- if (!current) {
- fprintf(stderr, "Error: for EndeffectorTwistController, tip is not connected to root\n");
- return false;
- }
- }
- // reverse order of joint vector
- std::reverse(joints_.begin(), joints_.end());
-
-
- fprintf(stderr, "initialized twist controller\n");
-
- return true;
-}
-
-
-
-
-
-
-void EndeffectorTwistController::update()
-{
- // get current time
- double time = robot_->hw_->current_time_;
-
- // check if joints are calibrated
- for (unsigned int i = 0; i < joints_.size(); ++i) {
- if (!joints_[i]->calibrated_)
- return;
- }
-
- // get the joint positions and velocities
- JntArrayVel jnt_posvel(num_joints_);
- for (unsigned int i=0; i<num_joints_; i++){
- jnt_posvel.q(i) = joints_[i]->position_;
- jnt_posvel.qdot(i) = joints_[i]->velocity_;
- }
-
- // get endeffector twist error
- FrameVel twist;
- jnt_to_twist_solver_->JntToCart(jnt_posvel, twist);
- twist_meas_ = twist.deriv();
- Twist error = twist_meas_ - twist_desi_;
- double dt = time - last_time_;
-
- // pid feedback
- for (unsigned int i=0; i<6; i++)
- wrench_out_(i) = pid_controller_[i].updatePid(error(i), dt);
-
- // send wrench to wrench controller
- wrench_controller_.wrench_desi_ = wrench_out_;
- wrench_controller_.update();
-
- // remember time
- last_time_ = time;
-}
-
-
-
-
-
-
-
-
-ROS_REGISTER_CONTROLLER(EndeffectorTwistControllerNode)
-
-EndeffectorTwistControllerNode::~EndeffectorTwistControllerNode()
-{
- ros::Node *node = ros::Node::instance();
-
- node->unsubscribe(topic_ + "/command");
- node->unsubscribe("spacenav/joy");
-}
-
-
-bool EndeffectorTwistControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- // get name of topic_ to listen to
- ros::Node *node = ros::Node::instance();
- topic_ = config->Attribute("topic") ? config->Attribute("topic") : "";
- if (topic_ == "") {
- fprintf(stderr, "No topic given to EndeffectorTwistControllerNode\n");
- return false;
- }
-
- // get parameters
- node->param("arm_twist/joystick_max_trans", joystick_max_trans_, 0.0);
- node->param("arm_twist/joystick_max_rot", joystick_max_rot_, 0.0);
-
- // initialize controller
- if (!controller_.initXml(robot, config))
- return false;
-
- // subscribe to twist commands
- node->subscribe(topic_ + "/command", twist_msg_,
- &EndeffectorTwistControllerNode::command, this, 1);
-
- // subscribe to joystick commands
- node->subscribe("spacenav/joy", joystick_msg_,
- &EndeffectorTwistControllerNode::joystick, this, 1);
-
- return true;
-}
-
-
-void EndeffectorTwistControllerNode::update()
-{
- controller_.update();
-}
-
-
-void EndeffectorTwistControllerNode::command()
-{
- // convert to twist command
- controller_.twist_desi_.vel(0) = twist_msg_.vel.x;
- controller_.twist_desi_.vel(1) = twist_msg_.vel.y;
- controller_.twist_desi_.vel(2) = twist_msg_.vel.z;
- controller_.twist_desi_.rot(0) = twist_msg_.rot.x;
- controller_.twist_desi_.rot(1) = twist_msg_.rot.y;
- controller_.twist_desi_.rot(2) = twist_msg_.rot.z;
-}
-
-
-void EndeffectorTwistControllerNode::joystick()
-{
- // convert to twist command
- for (unsigned int i=0; i<3; i++){
- controller_.twist_desi_.vel(i) = joystick_msg_.axes[i] * joystick_max_trans_;
- controller_.twist_desi_.rot(i) = joystick_msg_.axes[i+3] * joystick_max_rot_;
- }
-}
-
-
-}; // namespace
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_wrench_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_wrench_controller.cpp 2009-01-21 02:32:05 UTC (rev 9850)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_wrench_controller.cpp 2009-01-21 02:36:21 UTC (rev 9851)
@@ -1,234 +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 "urdf/parser.h"
-#include <algorithm>
-#include "robot_kinematics/robot_kinematics.h"
-#include "robot_mechanism_controllers/endeffector_wrench_controller.h"
-
-
-static const double JOYSTICK_MAX_FORCE = 20.0;
-static const double JOYSTICK_MAX_TORQUE = 0.75;
-
-
-using namespace KDL;
-
-namespace controller {
-
-ROS_REGISTER_CONTROLLER(EndeffectorWrenchController)
-
-EndeffectorWrenchController::EndeffectorWrenchController()
-: jnt_to_jac_solver_(NULL),
- joints_(0,(mechanism::JointState*)NULL)
-{
- printf("EndeffectorWrenchController constructor\n");
-}
-
-
-
-EndeffectorWrenchController::~EndeffectorWrenchController()
-{
- if (jnt_to_jac_solver_) delete jnt_to_jac_solver_;
-}
-
-
-
-bool EndeffectorWrenchController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- // set disired wrench to 0
- for (unsigned int i=0; i<3; i++){
- wrench_desi_.force(i) = 0;
- wrench_desi_.torque(i) = 0;
- }
-
- // parse robot description from xml file
- ros::Node *node = ros::Node::instance();
- robot_kinematics::RobotKinematics robot_kinematics ;
- string robot_desc;
- node->param("robotdesc/pr2", robot_desc, string("")) ;
- robot_kinematics.loadString(robot_desc.c_str()) ;
- robot_kinematics::SerialChain* serial_chain = robot_kinematics.getSerialChain("right_arm");
- if (serial_chain == NULL)
- fprintf(stderr, "Got NULL Chain\n") ;
-
- // convert description to KDL chain
- chain_ = serial_chain->chain;
- num_joints_ = chain_.getNrOfJoints();
- num_segments_ = chain_.getNrOfSegments();
- printf("Extracted KDL Chain with %u Joints and %u segments\n", num_joints_, num_segments_ );
- jnt_to_jac_solver_ = new ChainJntToJacSolver(chain_);
-
- // get chain
- TiXmlElement *chain = config->FirstChildElement("chain");
- if (!chain) {
- fprintf(stderr, "Error: EndeffectorWrenchController was not given a chain\n");
- return false;
- }
-
- // get names for root and tip of robot
- const char *root_name = chain->Attribute("root");
- const char *tip_name = chain->Attribute("tip");
- if (!root_name) {
- fprintf(stderr, "Error: Chain element for EndeffectorWrenchController must specify the root\n");
- return false;
- }
- if (!tip_name) {
- fprintf(stderr, "Error: Chain element for EndeffectorWrenchController must specify the tip\n");
- return false;
- }
-
- // test if we can get root from robot
- assert(robot);
- if (!robot->getLinkState(root_name)) {
- fprintf(stderr, "Error: link \"%s\" does not exist (EndeffectorWrenchController)\n", root_name);
- return false;
- }
-
- // get tip from robot
- mechanism::LinkState *current = robot->getLinkState(tip_name);
- if (!current) {
- fprintf(stderr, "Error: link \"%s\" does not exist (EndeffectorWrenchController)\n", tip_name);
- return false;
- }
-
- // Works up the chain, from the tip to the root, and get joints
- while (current->link_->name_ != std::string(root_name))
- {
- // get joint from current link
- joints_.push_back(robot->getJointState(current->link_->joint_name_));
- assert(joints_[joints_.size()-1]);
-
- // get parent link
- current = robot->getLinkState(current->link_->parent_name_);
-
- if (!current) {
- fprintf(stderr, "Error: for EndeffectorWrenchController, tip is not connected to root\n");
- return false;
- }
- }
- // reverse order of joint vector
- std::reverse(joints_.begin(), joints_.end());
-
- // get control parameters
-
-
-
- return true;
-}
-
-
-
-
-void EndeffectorWrenchController::update()
-{
- // check if joints are calibrated
- for (unsigned int i = 0; i < joints_.size(); ++i) {
- if (!joints_[i]->calibrated_)
- return;
- }
-
- // get the joint positions
- JntArray jnt_pos(num_joints_);
- for (unsigned int i=0; i<num_joints_; i++)
- jnt_pos(i) = joints_[i]->position_;
-
- // get the chain jacobian
- Jacobian jacobian(num_joints_, num_segments_);
- jnt_to_jac_solver_->JntToJac(jnt_pos, jacobian);
-
- // convert the wrench into joint torques
- JntArray jnt_torq(num_joints_);
- for (unsigned int i=0; i<num_joints_; i++){
- jnt_torq(i) = 0;
- for (unsigned int j=0; j<6; j++)
- jnt_torq(i) += (jacobian(j,i) * wrench_desi_(j));
- joints_[i]->commanded_effort_ = jnt_torq(i);
- }
-}
-
-
-
-
-
-
-
-
-
-ROS_REGISTER_CONTROLLER(EndeffectorWrenchControllerNode)
-
-EndeffectorWrenchControllerNode::~EndeffectorWrenchControllerNode()
-{
- ros::Node *node = ros::Node::instance();
- node->unsubscribe(topic_ + "/command");
-}
-
-bool EndeffectorWrenchControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- // get name of topic to listen to from xml file
- ros::Node *node = ros::Node::instance();
- topic_ = config->Attribute("topic") ? config->Attribute("topic") : "";
- if (topic_ == "") {
- fprintf(stderr, "No topic given to EndeffectorWrenchControllerNode\n");
- return false;
- }
-
- // initialize controller
- if (!controller_.initXml(robot, config))
- return false;
-
- // subscribe to wrench commands
- node->subscribe(topic_ + "/command", wrench_msg_,
- &EndeffectorWrenchControllerNode::command, this, 1);
-
- return true;
-}
-
-
-void EndeffectorWrenchControllerNode::update()
-{
- controller_.update();
-}
-
-
-void EndeffectorWrenchControllerNode::command()
-{
- // convert to wrench command
- controller_.wrench_desi_.force(0) = wrench_msg_.force.x;
- controller_.wrench_desi_.force(1) = wrench_msg_.force.y;
- controller_.wrench_desi_.force(2) = wrench_msg_.force.z;
- controller_.wrench_desi_.torque(0) = wrench_msg_.torque.x;
- controller_.wrench_desi_.torque(1) = wrench_msg_.torque.y;
- controller_.wrench_desi_.torque(2) = wrench_msg_.torque.z;
-}
-
-}; // namespace
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|