Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[19440] pkg/trunk
|
From: <stu...@us...> - 2009-07-22 21:38:09
|
Revision: 19440
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19440&view=rev
Author: stuglaser
Date: 2009-07-22 21:38:04 +0000 (Wed, 22 Jul 2009)
Log Message:
-----------
Experimental controllers package
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/sandbox/experimental_controllers/
pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt
pkg/trunk/sandbox/experimental_controllers/Makefile
pkg/trunk/sandbox/experimental_controllers/include/
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h
pkg/trunk/sandbox/experimental_controllers/mainpage.dox
pkg/trunk/sandbox/experimental_controllers/manifest.xml
pkg/trunk/sandbox/experimental_controllers/msg/
pkg/trunk/sandbox/experimental_controllers/msg/PlugInternalState.msg
pkg/trunk/sandbox/experimental_controllers/src/
pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp
Removed Paths:
-------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/msg/PlugInternalState.msg
pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
Modified: pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-07-22 21:37:16 UTC (rev 19439)
+++ pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-07-22 21:38:04 UTC (rev 19440)
@@ -6,7 +6,6 @@
genmsg()
gensrv()
set(_srcs
- src/plug_controller.cpp
src/cartesian_tff_controller.cpp
src/cartesian_trajectory_controller.cpp
src/cartesian_pose_controller.cpp
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-07-22 21:37:16 UTC (rev 19439)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-07-22 21:38:04 UTC (rev 19440)
@@ -1,182 +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 PLUG_CONTROLLER_H
-#define PLUG_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 "ros/node.h"
-#include "robot_msgs/Wrench.h"
-#include "robot_msgs/PoseStamped.h"
-#include "robot_msgs/Transform.h"
-#include "robot_mechanism_controllers/PlugInternalState.h"
-#include "robot_srvs/SetPoseStamped.h"
-#include "control_toolbox/pid.h"
-#include "misc_utils/subscription_guard.h"
-#include "mechanism_model/controller.h"
-#include "tf/transform_datatypes.h"
-#include "tf/transform_listener.h"
-#include "misc_utils/advertised_service_guard.h"
-#include "realtime_tools/realtime_publisher.h"
-
-#include "Eigen/Geometry"
-#include "Eigen/LU"
-#include "Eigen/Core"
-
-#include <visualization_msgs/Marker.h>
-
-
-namespace controller {
-
-class PlugController : public Controller
-{
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- PlugController();
- ~PlugController();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- void update();
- void computeConstraintJacobian();
- void computeConstraintNullSpace();
-
- void setToolOffset(const tf::Transform &);
-
- std::string root_name_;
-
- // input of the controller
- KDL::Wrench wrench_desi_;
- Eigen::Matrix<float,6,1> task_wrench_;
- Eigen::Vector3f outlet_pt_;
- Eigen::Vector3f outlet_norm_; // this must be normalized
-
- KDL::Frame endeffector_frame_;
- KDL::Frame desired_frame_;
-
- mechanism::Chain chain_;
- KDL::Chain kdl_chain_;
-
- double dist_to_line_;
- double f_r_;
- double f_roll_;
- double f_pitch_;
- double f_yaw_;
- KDL::Twist pose_error_;
-
-private:
-
- mechanism::RobotState *robot_;
- std::string controller_name_;
- 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_;
-
- // some parameters to define the constraint
-
- double upper_arm_limit;
- double upper_arm_dead_zone;
- double f_r_max;
- double f_pose_max;
- double f_limit_max;
- double last_time_;
- bool initialized_;
-
-
-
-
- control_toolbox::Pid roll_pid_; /**< Internal PID controller. */
- control_toolbox::Pid pitch_pid_; /**< Internal PID controller. */
- control_toolbox::Pid yaw_pid_; /**< Internal PID controller. */
- control_toolbox::Pid line_pid_; /**< Internal PID controller. */
-};
-
-
-class PlugControllerNode : public Controller
-{
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
-
- PlugControllerNode();
- ~PlugControllerNode();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- void update();
- void command();
- void outletPose();
-
- bool setToolFrame(robot_srvs::SetPoseStamped::Request &req,
- robot_srvs::SetPoseStamped::Response &resp);
-
- private:
- std::string topic_;
- ros::Node *node_;
- PlugController controller_;
- SubscriptionGuard guard_command_;
- SubscriptionGuard guard_outlet_pose_;
- AdvertisedServiceGuard guard_set_tool_frame_;
-
- robot_msgs::Wrench wrench_msg_;
- robot_msgs::PoseStamped outlet_pose_msg_;
- unsigned int loop_count_;
-
- tf::TransformListener TF; /**< The transform for converting from point to head and tilt frames. */
- realtime_tools::RealtimePublisher <robot_msgs::Transform>* current_frame_publisher_;
- realtime_tools::RealtimePublisher <robot_mechanism_controllers::PlugInternalState>* internal_state_publisher_;
-
-};
-
-} // namespace
-
-
-#endif
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/msg/PlugInternalState.msg
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/msg/PlugInternalState.msg 2009-07-22 21:37:16 UTC (rev 19439)
+++ pkg/trunk/controllers/robot_mechanism_controllers/msg/PlugInternalState.msg 2009-07-22 21:38:04 UTC (rev 19440)
@@ -1,9 +0,0 @@
-float64 line_error
-float64 line_force_cmd
-float64 roll_error
-float64 roll_force_cmd
-float64 pitch_error
-float64 pitch_force_cmd
-float64 yaw_error
-float64 yaw_force_cmd
-
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-07-22 21:37:16 UTC (rev 19439)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-07-22 21:38:04 UTC (rev 19440)
@@ -1,499 +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
- */
-#include "robot_mechanism_controllers/plug_controller.h"
-#include <algorithm>
-#include "angles/angles.h"
-#include "urdf/parser.h"
-#include "tf_conversions/tf_kdl.h"
-#include "kdl/chainfksolverpos_recursive.hpp"
-#include "kdl/chainjnttojacsolver.hpp"
-
-#define DEBUG 0 // easy debugging
-
-static const double JOYSTICK_MAX_FORCE = 20.0;
-static const double JOYSTICK_MAX_TORQUE = 0.75;
-
-
-using namespace KDL;
-
-namespace controller {
-
-ROS_REGISTER_CONTROLLER(PlugController)
-
-PlugController::PlugController() :
- outlet_pt_(1, 0, 0),
- outlet_norm_(1,0,0),
- jnt_to_jac_solver_(NULL),
- jnt_to_pose_solver_(NULL),
- initialized_(false)
-{
- constraint_jac_.setZero();
- constraint_wrench_.setZero();
- constraint_force_.setZero();
- printf("PlugController constructor\n");
-}
-
-
-
-PlugController::~PlugController()
-{
-}
-
-
-
-bool PlugController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- assert(robot);
- robot_ = robot;
-
- controller_name_ = config->Attribute("name");
-
- TiXmlElement *chain = config->FirstChildElement("chain");
- if (!chain) {
- ROS_ERROR("PlugController was not given a chain");
- return false;
- }
-
- // Gets names for the root and tip of the chain
- const char *tip_name = chain->Attribute("tip");
- if (!chain->Attribute("root")) {
- ROS_ERROR("Chain element for PlugController must specify the root");
- return false;
- }
- if (!tip_name) {
- ROS_ERROR("Chain element for PlugController must specify the tip");
- return false;
- }
-
- root_name_ = chain->Attribute("root");
- if (!chain_.init(robot->model_, root_name_, tip_name))
- return false;
- chain_.toKDL(kdl_chain_);
-
- // some parameters
- ros::Node *node = ros::Node::instance();
- assert(node);
-
- node->param(controller_name_+"/upper_arm_limit" , upper_arm_limit , -1.52 ) ; /// upper arm pose limit
-
- node->param(controller_name_+"/f_r_max" , f_r_max , 150.0) ; /// max radial force of line constraint
- node->param(controller_name_+"/f_pose_max" , f_pose_max , 40.0) ; /// max pose force
- node->param(controller_name_+"/f_limit_max" , f_limit_max , 100.0) ; /// max upper arm limit force
- node->param(controller_name_+"/upper_arm_dead_zone", upper_arm_dead_zone, 0.05);
-
- roll_pid_.initParam(controller_name_+"/pose_pid");
- pitch_pid_.initParam(controller_name_+"/pose_pid");
- yaw_pid_.initParam(controller_name_+"/pose_pid");
- line_pid_.initParam(controller_name_+"/line_pid");
- last_time_ = robot->model_->hw_->current_time_;
-
-
- // Constructs solvers and allocates matrices.
- unsigned int num_joints = kdl_chain_.getNrOfJoints();
- jnt_to_jac_solver_.reset(new ChainJntToJacSolver(kdl_chain_));
- jnt_to_pose_solver_.reset(new ChainFkSolverPos_recursive(kdl_chain_));
- task_jac_ = Eigen::MatrixXf::Zero(6, num_joints);
- identity_ = Eigen::MatrixXf::Identity(6, 6);
- identity_joint_ = Eigen::MatrixXf::Identity(num_joints, num_joints);
- constraint_null_space_ = Eigen::MatrixXf::Zero(6, 6);
- joint_constraint_force_= Eigen::MatrixXf::Zero(num_joints, 1);
- joint_constraint_jac_= Eigen::MatrixXf::Zero(num_joints, 1);
- joint_constraint_null_space_ = Eigen::MatrixXf::Zero(num_joints, num_joints);
- constraint_torq_ = Eigen::MatrixXf::Zero(num_joints, 1);
- task_torq_ = Eigen::MatrixXf::Zero(num_joints, 1);
- task_wrench_ = Eigen::Matrix<float,6,1>::Zero();
-
- // Sets desired wrench to 0
- for (unsigned int i=0; i<6; i++){
- task_wrench_(i) = 0;
- }
-
- return true;
-}
-
-
-void PlugController::update()
-{
- if (!chain_.allCalibrated(robot_->joint_states_))
- return;
-
- // Gets the joint positions
- JntArray jnt_pos(kdl_chain_.getNrOfJoints());
- chain_.getPositions(robot_->joint_states_, jnt_pos);
-
- // Grabs the inital pose of the robot for testing...
- if(!initialized_)
- {
- jnt_to_pose_solver_->JntToCart(jnt_pos, desired_frame_);
- for (unsigned int i = 0; i < 3; ++i)
- outlet_pt_[i] = desired_frame_.p[i];
- initialized_ = true;
- }
-
- Jacobian jacobian(kdl_chain_.getNrOfJoints(), kdl_chain_.getNrOfSegments());
- jnt_to_jac_solver_->JntToJac(jnt_pos, jacobian);
-
- // TODO: Write a function for doing this conversion
- //convert to eigen for easier math
- for (unsigned int i = 0; i < 6; i++)
- {
- for (unsigned int j = 0; j < kdl_chain_.getNrOfJoints(); j++)
- {
- task_jac_(i,j) = jacobian(i,j);
- }
- }
-
- // get endeffector pose
- jnt_to_pose_solver_->JntToCart(jnt_pos, endeffector_frame_);
-
- // compute the constraint jacobian and the constraint force
- computeConstraintJacobian();
-
- // compute the constraint wrench to apply
- constraint_wrench_ = constraint_jac_ * constraint_force_;
-
- // compute the constraint null space to project
- computeConstraintNullSpace();
-
- // convert the wrench into joint torques
- joint_constraint_torq_ = joint_constraint_force_;
- constraint_torq_ = joint_constraint_null_space_ * task_jac_.transpose() * constraint_wrench_;
- task_torq_ = joint_constraint_null_space_ * task_jac_.transpose() * constraint_null_space_ * task_wrench_;
-
-
-
- JntArray jnt_eff(kdl_chain_.getNrOfJoints());
- for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); ++i)
- jnt_eff(i) = joint_constraint_torq_(i) + constraint_torq_(i) + task_torq_(i);
- if (jnt_eff.rows() != 7)
- ROS_ERROR("Wrong number of joint effort elements: %d", jnt_eff.rows());
- else
- chain_.setEfforts(jnt_eff, robot_->joint_states_);
-}
-
-
-void PlugController::computeConstraintJacobian()
-{
- double time = robot_->model_->hw_->current_time_;
- // Clear force vector
- f_r_ = 0;
- f_roll_ = 0;
- f_pitch_ = 0;
- f_yaw_ = 0;
-
- // this will be computed based on the tool position in space
- constraint_jac_(0,0) = 0; // line constraint
- constraint_jac_(1,0) = 0; // line constraint
- constraint_jac_(2,0) = 0; // line constraint
- constraint_jac_(0,1) = 0; // line constraint
- constraint_jac_(1,1) = 0; // line constraint
- constraint_jac_(2,1) = 0; // line constraint
- // the pose constraint is always on
- constraint_jac_(3,2) = 1; // roll
- constraint_jac_(4,3) = 1; // pitch
- constraint_jac_(5,4) = 1; // yaw
-
- joint_constraint_jac_(2) = 0;
-
- // put the end_effector point into eigen
- Eigen::Vector3f end_effector_pt(endeffector_frame_.p(0), endeffector_frame_.p(1), endeffector_frame_.p(2));
-
- // Vector from the outlet to the end effector position
- Eigen::Vector3f v = end_effector_pt - outlet_pt_;
-
- // Vector from the end effector position to (the closest point on ) the line constraint.
- Eigen::Vector3f r_to_line = v.dot(outlet_norm_) * outlet_norm_ - v;
- Eigen::Vector3f other_norm = r_to_line.cross(outlet_norm_);
- other_norm = other_norm.normalized();
- dist_to_line_ = r_to_line.norm();
- r_to_line = r_to_line.normalized();
-
- // update the jacobian for the line constraint
- if (dist_to_line_ > 0.1)
- {
- constraint_jac_(0,0) = r_to_line(0);
- constraint_jac_(1,0) = r_to_line(1);
- constraint_jac_(2,0) = r_to_line(2);
- constraint_jac_(0,1) = other_norm(0);
- constraint_jac_(1,1) = other_norm(1);
- constraint_jac_(2,1) = other_norm(2);
- f_r_ = line_pid_.updatePid(-dist_to_line_, time-last_time_);
- if (fabs(f_r_) > f_r_max)
- f_r_ = f_r_max * f_r_ / fabs(f_r_);
- }
- else
- {
- f_r_ = 0;
- }
-
- // compute the pose error using a twist
- pose_error_ = diff(endeffector_frame_, desired_frame_);
-
- //roll constraint
- if (fabs(pose_error_(3)) > 0)
- {
- f_roll_ = roll_pid_.updatePid(-pose_error_(3), time-last_time_);
- if (fabs(f_roll_) > f_pose_max)
- f_roll_ = f_pose_max * f_roll_ / fabs(f_roll_);
- }
- else
- {
- f_roll_ = 0;
- }
-
- //pitch constraint
- if (fabs(pose_error_(4)) > 0)
- {
- f_pitch_ = pitch_pid_.updatePid(-pose_error_(4), time-last_time_);
- if (fabs(f_pitch_) > f_pose_max)
- f_pitch_ = f_pose_max * f_pitch_ / fabs(f_pitch_);
- }
- else
- {
- f_pitch_ = 0;
- }
-
- //yaw constraint
- if (fabs(pose_error_(5)) > 0)
- {
- f_yaw_ = yaw_pid_.updatePid(-pose_error_(5), time-last_time_);
- if (fabs(f_yaw_) > f_pose_max)
- f_yaw_ = f_pose_max * f_yaw_ / fabs(f_yaw_);
- }
- else
- {
- f_yaw_ = 0;
- }
-
-
- //joint constraint force - stop the upper arm from going past -90 degrees (1.57 rad)
- JntArray jnt_pos(kdl_chain_.getNrOfJoints());
- chain_.getPositions(robot_->joint_states_, jnt_pos);
-
- double joint_e = angles::shortest_angular_distance(jnt_pos(2), upper_arm_limit);
- if(joint_e < upper_arm_dead_zone)
- {
- joint_constraint_jac_(2) = 0;//1;
- }
-
- if(joint_e < 0)
- {
- joint_constraint_force_(2) =0;// joint_e * f_limit_max;
- }
- else
- {
- joint_constraint_force_(2) = 0;
- }
-
- constraint_force_(0) = f_r_;
- constraint_force_(1) = 0;
- constraint_force_(2) = f_roll_;
- constraint_force_(3) = f_pitch_;
- constraint_force_(4) = f_yaw_;
- last_time_ = time;
-}
-
-void PlugController::computeConstraintNullSpace()
-{
- // Compute generalized inverse, this is the transpose as long as the constraints are
- // orthonormal to eachother. Will replace with QR method later.
- constraint_null_space_ = identity_ - constraint_jac_ * constraint_jac_.transpose();
- joint_constraint_null_space_ = identity_joint_ - joint_constraint_jac_ * joint_constraint_jac_.transpose();
-
-}
-
-void PlugController::setToolOffset(const tf::Transform &tool_offset)
-{
- KDL::Chain new_kdl_chain;
- chain_.toKDL(new_kdl_chain);
-
- KDL::Frame tool_frame;
- tf::TransformTFToKDL(tool_offset, tool_frame);
- new_kdl_chain.addSegment(KDL::Segment("ToolOffset", KDL::Joint("ToolOffset", KDL::Joint::None), tool_frame));
-
- kdl_chain_ = new_kdl_chain;
-
- jnt_to_jac_solver_.reset(new ChainJntToJacSolver(kdl_chain_));
- jnt_to_pose_solver_.reset(new ChainFkSolverPos_recursive(kdl_chain_));
-}
-
-
-
-ROS_REGISTER_CONTROLLER(PlugControllerNode)
-
-
-PlugControllerNode::PlugControllerNode()
-: node_(ros::Node::instance()), loop_count_(0), TF(*ros::Node::instance(),false,ros::Duration(10.0))
-{
- current_frame_publisher_=NULL;
- internal_state_publisher_=NULL;
-}
-
-
-PlugControllerNode::~PlugControllerNode()
-{
- current_frame_publisher_->stop();
- delete current_frame_publisher_;
- internal_state_publisher_->stop();
- delete internal_state_publisher_;
-}
-
-bool PlugControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- // get name of topic to listen to from xml file
- topic_ = config->Attribute("name") ? config->Attribute("name") : "";
- if (topic_ == "") {
- fprintf(stderr, "No name given to PlugControllerNode\n");
- return false;
- }
-
- // initialize controller
- if (!controller_.initXml(robot, config))
- return false;
-
- assert(node_);
- // subscribe to wrench commands
- node_->subscribe(topic_ + "/command", wrench_msg_,
- &PlugControllerNode::command, this, 1);
- guard_command_.set(topic_ + "/command");
- // subscribe to outlet pose commands
- node_->subscribe(topic_ + "/outlet_pose", outlet_pose_msg_,
- &PlugControllerNode::outletPose, this, 1);
- guard_outlet_pose_.set(topic_ + "/outlet_pose");
-
- if (current_frame_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
- delete current_frame_publisher_ ;
- current_frame_publisher_ = new realtime_tools::RealtimePublisher <robot_msgs::Transform> (topic_ + "/transform", 1) ;
-
- if (internal_state_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
- delete internal_state_publisher_ ;
- internal_state_publisher_ = new realtime_tools::RealtimePublisher <robot_mechanism_controllers::PlugInternalState> (topic_ + "/internal_state", 1) ;
-
- node_->advertiseService(topic_ + "/set_tool_frame", &PlugControllerNode::setToolFrame, this);
- guard_set_tool_frame_.set(topic_ + "/set_tool_frame");
-
- return true;
-}
-
-
-void PlugControllerNode::update()
-{
- controller_.update();
-
- static int count =0;
- if (count % 100 == 0)
- {
- if (current_frame_publisher_->trylock())
- {
- tf::Transform transform;
- tf::TransformKDLToTF(controller_.endeffector_frame_, transform);
- tf::transformTFToMsg(transform, current_frame_publisher_->msg_);
- current_frame_publisher_->unlockAndPublish() ;
- }
- if (internal_state_publisher_->trylock())
- {
- internal_state_publisher_->msg_.line_error = controller_.dist_to_line_;
- internal_state_publisher_->msg_.line_force_cmd = controller_.f_r_;
- internal_state_publisher_->msg_.roll_error = controller_.pose_error_(3);
- internal_state_publisher_->msg_.roll_force_cmd = controller_.f_roll_;
- internal_state_publisher_->msg_.pitch_error = controller_.pose_error_(4);
- internal_state_publisher_->msg_.pitch_force_cmd = controller_.f_pitch_;
- internal_state_publisher_->msg_.yaw_error = controller_.pose_error_(5);
- internal_state_publisher_->msg_.yaw_force_cmd = controller_.f_yaw_;
- internal_state_publisher_->unlockAndPublish() ;
- }
- }
-
-}
-
-void PlugControllerNode::outletPose()
-{
- robot_msgs::PoseStamped outlet_in_root_;
- try
- {
- TF.transformPose(controller_.root_name_, outlet_pose_msg_, outlet_in_root_);
- }
- catch(tf::TransformException& ex)
- {
- ROS_WARN("Transform Exception %s", ex.what());
- return;
- }
- tf::Pose outlet;
- tf::poseMsgToTF(outlet_in_root_.pose, outlet);
-
- controller_.outlet_pt_(0) = outlet.getOrigin().x();
- controller_.outlet_pt_(1) = outlet.getOrigin().y();
- controller_.outlet_pt_(2) = outlet.getOrigin().z();
-
- tf::Vector3 norm = quatRotate(outlet.getRotation(), tf::Vector3(1.0, 0.0, 0.0));
- controller_.outlet_norm_(0) = norm.x();
- controller_.outlet_norm_(1) = norm.y();
- controller_.outlet_norm_(2) = norm.z();
-
- tf::TransformTFToKDL(outlet, controller_.desired_frame_);
-}
-
-void PlugControllerNode::command()
-{
- // convert to wrench command
- controller_.task_wrench_(0) = wrench_msg_.force.x;
- controller_.task_wrench_(1) = wrench_msg_.force.y;
- controller_.task_wrench_(2) = wrench_msg_.force.z;
- controller_.task_wrench_(3) = wrench_msg_.torque.x;
- controller_.task_wrench_(4) = wrench_msg_.torque.y;
- controller_.task_wrench_(5) = wrench_msg_.torque.z;
-
-}
-
-bool PlugControllerNode::setToolFrame(robot_srvs::SetPoseStamped::Request &req,
- robot_srvs::SetPoseStamped::Response &resp)
-{
- robot_msgs::PoseStamped tool_offset_msg;
- try
- {
- //TF.transformPose(controller_.chain_.getLinkName(), req.p, tool_offset_msg);
- TF.transformPose(controller_.kdl_chain_.getSegment(controller_.kdl_chain_.getNrOfSegments()-1).getName(), req.p, tool_offset_msg);
- }
- catch(tf::TransformException& ex)
- {
- ROS_WARN("Transform Exception %s", ex.what());
- return true;
- }
-
- tf::Transform tool_offset;
- tf::poseMsgToTF(tool_offset_msg.pose, tool_offset);
- controller_.setToolOffset(tool_offset);
- return true;
-}
-
-}; // namespace
Added: pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt (rev 0)
+++ pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt 2009-07-22 21:38:04 UTC (rev 19440)
@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rospack(experimental_controllers)
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+genmsg()
+#uncomment if you have defined services
+gensrv()
+
+rospack_add_library(experimental_controllers
+ src/plug_controller.cpp
+)
+target_link_libraries(experimental_controllers ltdl)
Added: pkg/trunk/sandbox/experimental_controllers/Makefile
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/Makefile (rev 0)
+++ pkg/trunk/sandbox/experimental_controllers/Makefile 2009-07-22 21:38:04 UTC (rev 19440)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Copied: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h (from rev 19438, pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h)
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h (rev 0)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h 2009-07-22 21:38:04 UTC (rev 19440)
@@ -0,0 +1,182 @@
+/*
+ * 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 PLUG_CONTROLLER_H
+#define PLUG_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 "ros/node.h"
+#include "robot_msgs/Wrench.h"
+#include "robot_msgs/PoseStamped.h"
+#include "robot_msgs/Transform.h"
+#include "experimental_controllers/PlugInternalState.h"
+#include "robot_srvs/SetPoseStamped.h"
+#include "control_toolbox/pid.h"
+#include "misc_utils/subscription_guard.h"
+#include "mechanism_model/controller.h"
+#include "tf/transform_datatypes.h"
+#include "tf/transform_listener.h"
+#include "misc_utils/advertised_service_guard.h"
+#include "realtime_tools/realtime_publisher.h"
+
+#include "Eigen/Geometry"
+#include "Eigen/LU"
+#include "Eigen/Core"
+
+#include <visualization_msgs/Marker.h>
+
+
+namespace controller {
+
+class PlugController : public Controller
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+ PlugController();
+ ~PlugController();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ void update();
+ void computeConstraintJacobian();
+ void computeConstraintNullSpace();
+
+ void setToolOffset(const tf::Transform &);
+
+ std::string root_name_;
+
+ // input of the controller
+ KDL::Wrench wrench_desi_;
+ Eigen::Matrix<float,6,1> task_wrench_;
+ Eigen::Vector3f outlet_pt_;
+ Eigen::Vector3f outlet_norm_; // this must be normalized
+
+ KDL::Frame endeffector_frame_;
+ KDL::Frame desired_frame_;
+
+ mechanism::Chain chain_;
+ KDL::Chain kdl_chain_;
+
+ double dist_to_line_;
+ double f_r_;
+ double f_roll_;
+ double f_pitch_;
+ double f_yaw_;
+ KDL::Twist pose_error_;
+
+private:
+
+ mechanism::RobotState *robot_;
+ std::string controller_name_;
+ 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_;
+
+ // some parameters to define the constraint
+
+ double upper_arm_limit;
+ double upper_arm_dead_zone;
+ double f_r_max;
+ double f_pose_max;
+ double f_limit_max;
+ double last_time_;
+ bool initialized_;
+
+
+
+
+ control_toolbox::Pid roll_pid_; /**< Internal PID controller. */
+ control_toolbox::Pid pitch_pid_; /**< Internal PID controller. */
+ control_toolbox::Pid yaw_pid_; /**< Internal PID controller. */
+ control_toolbox::Pid line_pid_; /**< Internal PID controller. */
+};
+
+
+class PlugControllerNode : public Controller
+{
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ PlugControllerNode();
+ ~PlugControllerNode();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ void update();
+ void command();
+ void outletPose();
+
+ bool setToolFrame(robot_srvs::SetPoseStamped::Request &req,
+ robot_srvs::SetPoseStamped::Response &resp);
+
+ private:
+ std::string topic_;
+ ros::Node *node_;
+ PlugController controller_;
+ SubscriptionGuard guard_command_;
+ SubscriptionGuard guard_outlet_pose_;
+ AdvertisedServiceGuard guard_set_tool_frame_;
+
+ robot_msgs::Wrench wrench_msg_;
+ robot_msgs::PoseStamped outlet_pose_msg_;
+ unsigned int loop_count_;
+
+ tf::TransformListener TF; /**< The transform for converting from point to head and tilt frames. */
+ realtime_tools::RealtimePublisher <robot_msgs::Transform>* current_frame_publisher_;
+ realtime_tools::RealtimePublisher <experimental_controllers::PlugInternalState>* internal_state_publisher_;
+
+};
+
+} // namespace
+
+
+#endif
Added: pkg/trunk/sandbox/experimental_controllers/mainpage.dox
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/mainpage.dox (rev 0)
+++ pkg/trunk/sandbox/experimental_controllers/mainpage.dox 2009-07-22 21:38:04 UTC (rev 19440)
@@ -0,0 +1,119 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b experimental_controllers is ...
+
+<!--
+In addition to providing an overview of your package,
+this is the section where the specification and design/architecture
+should be detailed. While the original specification may be done on the
+wiki, it should be transferred here once your package starts to take shape.
+You can then link to this documentation page from the Wiki.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+\section rosapi ROS API
+
+<!--
+Names are very important in ROS because they can be remapped on the
+command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
+APPEAR IN THE CODE. You should list names of every topic, service and
+parameter used in your code. There is a template below that you can
+use to document each node separately.
+
+List of nodes:
+- \b node_name1
+- \b node_name2
+-->
+
+<!-- START: copy from here to 'END' for each node
+
+<hr>
+
+\subsection node_name node_name
+
+node_name does (provide a basic description of your node)
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ node_type1
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "in": [std_msgs/FooType] description of in
+
+Publishes to:
+- \b "out": [std_msgs/FooType] description of out
+
+
+\subsubsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+- \b "~my_param" : \b [string] description of my_param
+
+Sets the following parameters on the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+
+
+END: copy for each node -->
+
+
+<!-- START: Uncomment if you have any command-line tools
+
+\section commandline Command-line tools
+
+This section is a catch-all for any additional tools that your package
+provides or uses that may be of use to the reader. For example:
+
+- tools/scripts (e.g. rospack, roscd)
+- roslaunch .launch files
+- xmlparam files
+
+\subsection script_name script_name
+
+Description of what this script/file does.
+
+\subsubsection Usage
+\verbatim
+$ ./script_name [args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ ./script_name foo bar
+\endverbatim
+
+END: Command-Line Tools Section -->
+
+*/
\ No newline at end of file
Added: pkg/trunk/sandbox/experimental_controllers/manifest.xml
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/manifest.xml (rev 0)
+++ pkg/trunk/sandbox/experimental_controllers/manifest.xml 2009-07-22 21:38:04 UTC (rev 19440)
@@ -0,0 +1,41 @@
+<package>
+ <description brief="experimental_controllers">
+ Controllers that are still under development
+ </description>
+ <author>Stuart Glaser</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/experimental_controllers</url>
+
+ <depend package="kdl_parser"/>
+ <depend package="rospy"/>
+ <depend package="mechanism_model" />
+ <depend package="mechanism_control" />
+ <depend package="control_toolbox" />
+ <depend package="tinyxml" />
+ <depend package="misc_utils" />
+ <depend package="realtime_tools" />
+ <depend package="roscpp" />
+ <depend package="std_msgs" />
+ <depend package="robot_srvs" />
+ <depend package="robot_msgs" />
+ <depend package="manipulation_msgs" />
+ <depend package="visualization_msgs" />
+ <depend package="tf" />
+ <depend package="tf_conversions" />
+ <depend package="kdl" />
+ <depend package="angles" />
+ <depend package="joy" />
+ <depend package="eigen" />
+ <depend package="filters" />
+
+ <export>
+ <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp"
+ lflags="-Wl,-rpath,${prefix}/lib/ -L${prefix}/lib -lexperimental_controllers" />
+ </export>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libltdl3-dev"/>
+ <rosdep name="libtool"/>
+
+</package>
+
+
Copied: pkg/trunk/sandbox/experimental_controllers/msg/PlugInternalState.msg (from rev 19438, pkg/trunk/controllers/robot_mechanism_controllers/msg/PlugInternalState.msg)
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/msg/PlugInternalState.msg (rev 0)
+++ pkg/trunk/sandbox/experimental_controllers/msg/PlugInternalState.msg 2009-07-22 21:38:04 UTC (rev 19440)
@@ -0,0 +1,9 @@
+float64 line_error
+float64 line_force_cmd
+float64 roll_error
+float64 roll_force_cmd
+float64 pitch_error
+float64 pitch_force_cmd
+float64 yaw_error
+float64 yaw_force_cmd
+
Copied: pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp (from rev 19438, pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp)
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp (rev 0)
+++ pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp 2009-07-22 21:38:04 UTC (rev 19440)
@@ -0,0 +1,499 @@
+/*
+ * 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
+ */
+#include "experimental_controllers/plug_controller.h"
+#include <algorithm>
+#include "angles/angles.h"
+#include "urdf/parser.h"
+#include "tf_conversions/tf_kdl.h"
+#include "kdl/chainfksolverpos_recursive.hpp"
+#include "kdl/chainjnttojacsolver.hpp"
+
+#define DEBUG 0 // easy debugging
+
+static const double JOYSTICK_MAX_FORCE = 20.0;
+static const double JOYSTICK_MAX_TORQUE = 0.75;
+
+
+using namespace KDL;
+
+namespace controller {
+
+ROS_REGISTER_CONTROLLER(PlugController)
+
+PlugController::PlugController() :
+ outlet_pt_(1, 0, 0),
+ outlet_norm_(1,0,0),
+ jnt_to_jac_solver_(NULL),
+ jnt_to_pose_solver_(NULL),
+ initialized_(false)
+{
+ constraint_jac_.setZero();
+ constraint_wrench_.setZero();
+ constraint_force_.setZero();
+ printf("PlugController constructor\n");
+}
+
+
+
+PlugController::~PlugController()
+{
+}
+
+
+
+bool PlugController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ assert(robot);
+ robot_ = robot;
+
+ controller_name_ = config->Attribute("name");
+
+ TiXmlElement *chain = config->FirstChildElement("chain");
+ if (!chain) {
+ ROS_ERROR("PlugController was not given a chain");
+ return false;
+ }
+
+ // Gets names for the root and tip of the chain
+ const char *tip_name = chain->Attribute("tip");
+ if (!chain->Attribute("root")) {
+ ROS_ERROR("Chain element for PlugController must specify the root");
+ return false;
+ }
+ if (!tip_name) {
+ ROS_ERROR("Chain element for PlugController must specify the tip");
+ return false;
+ }
+
+ root_name_ = chain->Attribute("root");
+ if (!chain_.init(robot->model_, root_name_, tip_name))
+ return false;
+ chain_.toKDL(kdl_chain_);
+
+ // some parameters
+ ros::Node *node = ros::Node::instance();
+ assert(node);
+
+ node->param(controller_name_+"/upper_arm_limit" , upper_arm_limit , -1.52 ) ; /// upper arm pose limit
+
+ node->param(controller_name_+"/f_r_max" , f_r_max , 150.0) ; /// max radial force of line constraint
+ node->param(controller_name_+"/f_pose_max" , f_pose_max , 40.0) ; /// max pose force
+ node->param(controller_name_+"/f_limit_max" , f_limit_max , 100.0) ; /// max upper arm limit force
+ node->param(controller_name_+"/upper_arm_dead_zone", upper_arm_dead_zone, 0.05);
+
+ roll_pid_.initParam(controller_name_+"/pose_pid");
+ pitch_pid_.initParam(controller_name_+"/pose_pid");
+ yaw_pid_.initParam(controller_name_+"/pose_pid");
+ line_pid_.initParam(controller_name_+"/line_pid");
+ last_time_ = robot->model_->hw_->current_time_;
+
+
+ // Constructs solvers and allocates matrices.
+ unsigned int num_joints = kdl_chain_.getNrOfJoints();
+ jnt_to_jac_solver_.reset(new ChainJntToJacSolver(kdl_chain_));
+ jnt_to_pose_solver_.reset(new ChainFkSolverPos_recursive(kdl_chain_));
+ task_jac_ = Eigen::MatrixXf::Zero(6, num_joints);
+ identity_ = Eigen::MatrixXf::Identity(6, 6);
+ identity_joint_ = Eigen::MatrixXf::Identity(num_joints, num_joints);
+ constraint_null_space_ = Eigen::MatrixXf::Zero(6, 6);
+ joint_constraint_force_= Eigen::MatrixXf::Zero(num_joints, 1);
+ joint_constraint_jac_= Eigen::MatrixXf::Zero(num_joints, 1);
+ joint_constraint_null_space_ = Eigen::MatrixXf::Zero(num_joints, num_joints);
+ constraint_torq_ = Eigen::MatrixXf::Zero(num_joints, 1);
+ task_torq_ = Eigen::MatrixXf::Zero(num_joints, 1);
+ task_wrench_ = Eigen::Matrix<float,6,1>::Zero();
+
+ // Sets desired wrench to 0
+ for (unsigned int i=0; i<6; i++){
+ task_wrench_(i) = 0;
+ }
+
+ return true;
+}
+
+
+void PlugController::update()
+{
+ if (!chain_.allCalibrated(robot_->joint_states_))
+ return;
+
+ // Gets the joint positions
+ JntArray jnt_pos(kdl_chain_.getNrOfJoints());
+ chain_.getPositions(robot_->joint_states_, jnt_pos);
+
+ // Grabs the inital pose of the robot for testing...
+ if(!initialized_)
+ {
+ jnt_to_pose_solver_->JntToCart(jnt_pos, desired_frame_);
+ for (unsigned int i = 0; i < 3; ++i)
+ outlet_pt_[i] = desired_frame_.p[i];
+ initialized_ = true;
+ }
+
+ Jacobian jacobian(kdl_chain_.getNrOfJoints(), kdl_chain_.getNrOfSegments());
+ jnt_to_jac_solver_->JntToJac(jnt_pos, jacobian);
+
+ // TODO: Write a function for doing this conversion
+ //convert to eigen for easier math
+ for (unsigned int i = 0; i < 6; i++)
+ {
+ for (unsigned int j = 0; j < kdl_chain_.getNrOfJoints(); j++)
+ {
+ task_jac_(i,j) = jacobian(i,j);
+ }
+ }
+
+ // get endeffector pose
+ jnt_to_pose_solver_->JntToCart(jnt_pos, endeffector_frame_);
+
+ // compute the constraint jacobian and the constraint force
+ computeConstraintJacobian();
+
+ // compute the constraint wrench to apply
+ constraint_wrench_ = constraint_jac_ * constraint_force_;
+
+ // compute the constraint null space to project
+ computeConstraintNullSpace();
+
+ // convert the wrench into joint torques
+ joint_constraint_torq_ = joint_constraint_force_;
+ constraint_torq_ = joint_constraint_null_space_ * task_jac_.transpose() * constraint_wrench_;
+ task_torq_ = joint_constraint_null_space_ * task_jac_.transpose() * constraint_null_space_ * task_wrench_;
+
+
+
+ JntArray jnt_eff(kdl_chain_.getNrOfJoints());
+ for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); ++i)
+ jnt_eff(i) = joint_constraint_torq_(i) + constraint_torq_(i) + task_torq_(i);
+ if (jnt_eff.rows() != 7)
+ ROS_ERROR("Wrong number of joint effort elements: %d", jnt_eff.rows());
+ else
+ chain_.setEfforts(jnt_eff, robot_->joint_states_);
+}
+
+
+void PlugController::computeConstraintJacobian()
+{
+ double time = robot_->model_->hw_->current_time_;
+ // Clear force vector
+ f_r_ = 0;
+ f_roll_ = 0;
+ f_pitch_ = 0;
+ f_yaw_ = 0;
+
+ // this will be computed based on the tool position in space
+ constraint_jac_(0,0) = 0; // line constraint
+ constraint_jac_(1,0) = 0; // line constraint
+ constraint_jac_(2,0) = 0; // line constraint
+ constraint_jac_(0,1) = 0; // line constraint
+ constraint_jac_(1,1) = 0; // line constraint
+ constraint_jac_(2,1) = 0; // line constraint
+ // the pose constraint is always on
+ constraint_jac_(3,2) = 1; // roll
+ constraint_jac_(4,3) = 1; // pitch
+ constraint_jac_(5,4) = 1; // yaw
+
+ joint_constraint_jac_(2) = 0;
+
+ // put the end_effector point into eigen
+ Eigen::Vector3f end_effector_pt(endeffector_frame_.p(0), endeffector_frame_.p(1), endeffector_frame_.p(2));
+
+ // Vector from the outlet to the end effector position
+ Eigen::Vector3f v = end_effector_pt - outlet_pt_;
+
+ // Vector from the end effector position to (the closest point on ) the line constraint.
+ Eigen::Vector3f r_to_line = v.dot(outlet_norm_) * outlet_norm_ - v;
+ Eigen::Vector3f other_norm = r_to_line.cross(outlet_norm_);
+ other_norm = other_norm.normalized();
+ dist_to_line_ = r_to_line.norm();
+ r_to_line = r_to_line.normalized();
+
+ // update the jacobian for the line constraint
+ if (dist_to_line_ > 0.1)
+ {
+ constraint_jac_(0,0) = r_to_line(0);
+ constraint_jac_(1,0) = r_to_line(1);
+ constraint_jac_(2,0) = r_to_line(2);
+ constraint_jac_(0,1) = other_norm(0);
+ constraint_jac_(1,1) = other_norm(1);
+ constraint_jac_(2,1) = other_norm(2);
+ f_r_ = line_pid_.updatePid(-dist_to_line_, time-last_time_);
+ if (fabs(f_r_) > f_r_max)
+ f_r_ = f_r_max * f_r_ / fabs(f_r_);
+ }
+ else
+ {
+ f_r_ = 0;
+ }
+
+ // compute the pose error using a twist
+ pose_error_ = diff(endeffector_frame_, desired_frame_);
+
+ //roll constraint
+ if (fabs(pose_error_(3)) > 0)
+ {
+ f_roll_ = roll_pid_.updatePid(-pose_error_(3), time-last_time_);
+ if (fabs(f_roll_) > f_pose_max)
+ f_roll_ = f_pose_max * f_roll_ / fabs(f_roll_);
+ }
+ else
+ {
+ f_roll_ = 0;
+ }
+
+ //pitch constraint
+ if (fabs(pose_error_(4)) > 0)
+ {
+ f_pitch_ = pitch_pid_.updatePid(-pose_error_(4), time-last_time_);
+ if (fabs(f_pitch_) > f_pose_max)
+ f_pitch_ = f_pose_max * f_pitch_ / fabs(f_pitch_);
+ }
+ else
+ {
+ f_pitch_ = 0;
+ }
+
+ //yaw constraint
+ if (fabs(pose_error_(5)) > 0)
+ {
+ f_yaw_ = yaw_pid_.updatePid(-pose_error_(5), time-last_time_);
+ if (fabs(f_yaw_) > f_pose_max)
+ f_yaw_ = f_pose_max * f_yaw_ / fabs(f_yaw_);
+ }
+ else
+ {
+ f_yaw_ = 0;
+ }
+
+
+ //joint constraint force - stop the upper arm from going past -90 degrees (1.57 rad)
+ JntArray jnt_pos(kdl_chain_.getNrOfJoints());
+ chain_.getPositions(robot_->joint_states_, jnt_pos);
+
+ double joint_e = angles::shortest_angular_distance(jnt_pos(2), upper_arm_limit);
+ if(joint_e < upper_arm_dead_zone)
+ {
+ joint_constraint_jac_(2) = 0;//1;
+ }
+
+ if(joint_e < 0)
+ {
+ joint_constraint_force_(2) =0;// joint_e * f_limit_max;
+ }
+ else
+ {
+ joint_constraint_force_(2) = 0;
+ }
+
+ constraint_force_(0) = f_r_;
+ constraint_force_(1) = 0;
+ constraint_force_(2) = f_roll_;
+ constraint_force_(3) = f_pitch_;
+ constraint_force_(4) = f_yaw_;
+ last_time_ = time;
+}
+
+void PlugController::computeConstraintNullSpace()
+{
+ // Compute generalized inverse, this is the transpose as long as the constraints are
+ // orthonormal to eachother. Will replace with QR method later.
+ constraint_null_space_ = identity_ - constraint_jac_ * constraint_jac_.transpose();
+ joint_constraint_null_space_ = identity_joint_ - joint_constraint_jac_ * joint_constraint_jac_.transpose();
+
+}
+
+void PlugController::setToolOffset(const tf::Transform &tool_offset)
+{
+ KDL::Chain new_kdl_chain;
+ chain_.toKDL(new_kdl_chain);
+
+ KDL::Frame tool_frame;
+ tf::TransformTFToKDL(tool_offset, tool_frame);
+ new_kdl_chain.addSegment(KDL::Segment("ToolOffset", KDL::Joint("ToolOffset", KDL::Joint::None), tool_frame));
+
+ kdl_chain_ = new_kdl_chain;
+
+ jnt_to_jac_solver_.reset(new ChainJntToJacSolver(kdl_chain_));
+ jnt_to_pose_solver_.reset(new ChainFkSolverPos_recursive(kdl_chain_));
+}
+
+
+
+ROS_REGISTER_CONTROLLER(PlugControllerNode)
+
+
+PlugControllerNode::PlugControllerNode()
+: node_(ros::Node::instance()), loop_count_(0), TF(*ros::Node::instance(),false,ros::Duration(10.0))
+{
+ current_frame_publisher_=NULL;
+ internal_state_publisher_=NULL;
+}
+
+
+PlugControllerNode::~PlugControllerNode()
+{
+ current_frame_publisher_->stop();
+ delete current_frame_publisher_;
+ internal_state_publisher_->stop();
+ delete internal_state_publisher_;
+}
+
+bool PlugControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ // get name of topic to listen to from xml file
+ topic_ = config->Attribute("name") ? config->Attribute("name") : "";
+ if (topic_ == "") {
+ fprintf(stderr, "No name given to PlugControllerNode\n");
+ return false;
+ }
+
+ // initialize controller
+ if (!controller_.initXml(robot, config))
+ return false;
+
+ assert(node_);
+ // subscribe to wrench commands
+ node_->subscribe(topic_ + "/command", wrench_msg_,
+ &PlugControllerNode::command, this, 1);
+ guard_command_.set(topic_ + "/command");
+ // subscribe to outlet pose commands
+ node_->subscribe(topic_ + "/outlet_pose", outlet_pose_msg_,
+ &PlugControllerNode::outletPose, this, 1);
+ guard_outlet_pose_.set(topic_ + "/outlet_pose");
+
+ if (current_frame_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+ delete current_frame_publisher_ ;
+ current_frame_publisher_ = new realtime_tools::RealtimePublisher <robot_msgs::Transform> (topic_ + "/transform", 1) ;
+
+ if (internal_state_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+ delete internal_state_publisher_ ;
+ internal_state_publisher_ = new realtime_tools::RealtimePublisher <experimental_controllers::PlugInternalState> (topic_ + "/internal_state", 1) ;
+
+ node_->advertiseService(topic_ + "/set_tool_frame", &PlugControllerNode::setToolFrame, this);
+ guard_set_tool_frame_.set(topic_ + "/set_tool_frame");
+
+ return true;
+}
+
+
+void PlugControllerNode::update()
+{
+ controller_.update();
+
+ static int count =0;
+ if (count % 100 == 0)
+ {
+ if (current_frame_publisher_->trylock())
+ {
+ tf::Transform transform;
+ tf::TransformKDLToTF(controller_.endeffector_frame_, transform);
+ tf::transformTFToMsg(transform, current_frame_publisher_->msg_);
+ current_frame_publisher_->unlockAndPublish() ;
+ }
+ if (internal_state_publisher_->trylock())
+ {
+ internal_state_publisher_->msg_.line_error = controller_.dist_to_line_;
+ internal_state_publisher_->msg_.line_force_cmd = controller_.f_r_;
+ internal_state_publisher_->msg_.roll_error = controller_.pose_error_(3);
+ internal_state_publisher_->msg_.roll_force_cmd = controller_.f_roll_;
+ internal_state_publisher_->msg_.pitch_error = controller_.pose_error_(4);
+ internal_state_publisher_->msg_.pitch_force_cmd = controller_.f_pitch_;
+ internal_state_publisher_->msg_.yaw_error = controller_.pose_error_(5);
+ internal_state_publisher_->msg_.yaw_force_cmd = controller_.f_yaw_;
+ internal_state_publisher_->unlockAndPublish() ;
+ }
+ }
+
+}
+
+void PlugControllerNode::outletPose()
+{
+ robot_msgs::PoseStamped outlet_in_root_;
+ try
+ {
+ TF.transformPose(controller_.root_name_, outlet_pose_msg_, outlet_in_root_);
+ }
+ catch(tf::TransformException& ex)
+ {
+ ROS_WARN("Transform Exception %s", ex.what());
+ return;
+ }
+ tf::Pose outlet;
+ tf::poseMsgToTF(outlet_in_root_.pose, outlet);
+
+ controller_.outlet_pt_(0) = outlet.getOrigin().x();
+ controller_.outlet_pt_(1) = outlet.getOrigin().y();
+ controller_.outlet_pt_(2) = outlet.getOrigin().z();
+
+ tf::Vector3 norm = quatRotate(outlet.getRotation(), tf::Vector3(1.0, 0.0, 0.0));
+ controller_.outlet_norm_(0) = norm.x();
+ controller_.outlet_norm_(1) = norm.y();
+ controller_.outlet_norm_(2) = norm.z();
+
+ tf::TransformTFToKDL(outlet, controller_.desired_frame_);
+}
+
+void PlugControllerNode::command()
+{
+ // convert to wrench command
+ controller_.task_wrench_(0) = wrench_msg_.force.x;
+ controller_.task_wrench_(1) = wrench_msg_.force.y;
+ controller_.task_wrench_(2) = wrench_msg_.force.z;
+ controller_.task_wrench_(3) = wrench_msg_.torque.x;
+ controller_.task_wrench_(4) = wrench_msg_.torque.y;
+ controller_.task_wrench_(5) = wrench_msg_.torque.z;
+
+}
+
+bool PlugControllerNode::setToolFrame(robot_srvs::SetPoseStamped::Request &req,
+ robot_srvs::SetPoseStamped::Response &resp)
+{
+ robot_msgs::PoseStamped tool_offset_msg;
+ try
+ {
+ //TF.transformPose(controller_.chain_.getLinkName(), req.p, tool_offset_msg);
+ TF.transformPose(controller_.kdl_chain_.getSegment(controller_.kdl_chain_.getNrOfSegments()-1).getName(), req.p, tool_offset_msg);
+ }
+ catch(tf::TransformException& ex)
+ {
+ ROS_WARN("Transform Exception %s", ex.what());
+ return true;
+ }
+
+ tf::Transform tool_offset;
+ tf::poseMsgToTF(tool_offset_msg.pose, tool_offset);
+ controller_.setToolOffset(tool_offset);
+ return true;
+}
+
+}; // namespace
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
