Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[14346] pkg/trunk
|
From: <stu...@us...> - 2009-04-23 20:09:34
|
Revision: 14346
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14346&view=rev
Author: stuglaser
Date: 2009-04-23 20:09:28 +0000 (Thu, 23 Apr 2009)
Log Message:
-----------
Broke dependency of robot_mechanism_controllers on robot_kinematics
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_tff_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller_ik.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_chain_constraint_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h 2009-04-23 20:09:28 UTC (rev 14346)
@@ -35,8 +35,11 @@
#define CARTESIAN_TWIST_CONTROLLER_H
#include <vector>
+#include <boost/scoped_ptr.hpp>
#include <kdl/chain.hpp>
#include <kdl/frames.hpp>
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chainiksolvervel_pinv.hpp>
#include <ros/node.h>
#include <robot_msgs/Twist.h>
#include <mechanism_model/controller.h>
@@ -44,7 +47,6 @@
#include <robot_mechanism_controllers/cartesian_wrench_controller.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
#include <control_toolbox/pid.h>
-#include <boost/scoped_ptr.hpp>
namespace controller {
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h 2009-04-23 20:09:28 UTC (rev 14346)
@@ -67,7 +67,7 @@
KDL::Wrench wrench_desi_;
private:
- bool publishDiagnostics(int level, const string& message);
+ bool publishDiagnostics(int level, const std::string& message);
ros::Node* node_;
std::string controller_name_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-04-23 20:09:28 UTC (rev 14346)
@@ -39,6 +39,7 @@
#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 "misc_utils/subscription_guard.h"
@@ -48,8 +49,6 @@
#include "joy/Joy.h"
#include "Eigen/LU"
#include "Eigen/Core"
-#include "robot_kinematics/robot_kinematics.h"
-
#include <robot_msgs/VisualizationMarker.h>
@@ -87,10 +86,10 @@
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 identity_joint_;
Eigen::MatrixXf constraint_null_space_;
Eigen::MatrixXf constraint_torq_;
Eigen::MatrixXf joint_constraint_torq_;
@@ -107,8 +106,8 @@
double f_x_max;
double f_r_max;
double f_pose_max;
- double f_limit_max;
-
+ double f_limit_max;
+
double desired_roll_;
double desired_pitch_;
double desired_yaw_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h 2009-04-23 20:09:28 UTC (rev 14346)
@@ -36,7 +36,7 @@
/*! \class controller::JointChainConstraintController
\brief Adds joint position constraints to any connected chain of joints.
- Wrenches are in the cartesian frame in the root frame on the tip frame.
+ Wrenches are in the cartesian frame in the root frame on the tip frame.
Example config:<br>
@@ -51,6 +51,9 @@
#define JOINT_CHAIN_CONSTRAINT_CONTROLLER_H
#include <vector>
+#include <cstdio>
+#include <cstring>
+#include <string>
#include <kdl/chain.hpp>
#include <kdl/frames.hpp>
#include <kdl/chainjnttojacsolver.hpp>
@@ -63,15 +66,11 @@
#include "mechanism_model/chain.h"
#include "tf/transform_datatypes.h"
-#include <stdio.h>
-#include <string.h>
#include "misc_utils/advertised_service_guard.h"
-
#include "Eigen/Geometry"
#include "Eigen/LU"
#include "Eigen/Core"
-#include "robot_kinematics/robot_kinematics.h"
namespace controller {
@@ -88,7 +87,7 @@
int joint_chain_index_;
double joint_error_;
bool remove_;
-
+
control_toolbox::Pid pid_;
};
@@ -97,7 +96,7 @@
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
-
+
JointChainConstraintController();
~JointChainConstraintController();
@@ -107,15 +106,15 @@
* \param tip_name The tip of the chain.
* \param *robot The robot.
*/
- bool init(mechanism::RobotState *robot, const std::string& root_name,
+ bool init(mechanism::RobotState *robot, const std::string& root_name,
const std::string& tip_name, const std::string& controller_name);
void update();
-
+
// input of the controller
KDL::Wrench wrench_desired_;
-
+
private:
void computeConstraintTorques();
void computeConstraintJacobian();
@@ -136,22 +135,22 @@
KDL::ChainJntToJacSolver *jnt_to_jac_solver_;
KDL::JntArray jnt_pos_, jnt_eff_;
KDL::Jacobian chain_kdl_jacobian_;
-
+
Eigen::MatrixXf identity_;
Eigen::MatrixXf chain_eigen_jacobian_;
Eigen::MatrixXf task_torque_;
-
+
// joint constraint matrices and vectors
Eigen::MatrixXf joint_constraint_torque_;
Eigen::MatrixXf joint_constraint_jacobian_;
Eigen::MatrixXf joint_constraint_null_space_;
-
+
double last_time_;
-
+
bool initialized_;
AdvertisedServiceGuard change_constraints_guard_;
-
+
};
@@ -176,7 +175,7 @@
robot_msgs::Wrench wrench_msg_;
-
+
};
} // namespace
Modified: 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-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-04-23 20:09:28 UTC (rev 14346)
@@ -39,26 +39,24 @@
#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 "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 <realtime_tools/realtime_publisher.h>
-
-
#include "Eigen/Geometry"
#include "Eigen/LU"
#include "Eigen/Core"
-#include "robot_kinematics/robot_kinematics.h"
#include <robot_msgs/VisualizationMarker.h>
@@ -80,7 +78,7 @@
void setToolOffset(const tf::Transform &);
std::string root_name_;
-
+
// input of the controller
KDL::Wrench wrench_desi_;
Eigen::Matrix<float,6,1> task_wrench_;
@@ -133,10 +131,10 @@
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. */
Modified: pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-04-23 20:09:28 UTC (rev 14346)
@@ -22,7 +22,6 @@
<depend package="joy" />
<depend package="eigen" />
<depend package="filters" />
- <depend package="robot_kinematics" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -34,8 +34,8 @@
#include <algorithm>
-#include <robot_kinematics/robot_kinematics.h>
#include <mechanism_control/mechanism_control.h>
+#include "kdl/chainfksolverpos_recursive.hpp"
#include "robot_mechanism_controllers/cartesian_pose_controller.h"
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_tff_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_tff_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_tff_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -31,10 +31,10 @@
* Author: Wim Meeussen
*/
+#include "robot_mechanism_controllers/cartesian_tff_controller.h"
#include <algorithm>
-#include <robot_kinematics/robot_kinematics.h>
#include <mechanism_control/mechanism_control.h>
-#include "robot_mechanism_controllers/cartesian_tff_controller.h"
+#include <kdl/chainfksolvervel_recursive.hpp>
using namespace KDL;
@@ -131,7 +131,7 @@
ROS_ERROR("CartesianTFFController: could not get instance to mechanism control");
return false;
}
- string output;
+ std::string output;
if (!node_->getParam(controller_name_+"/output", output)){
ROS_ERROR("CartesianTFFController: No ouptut name found on parameter server");
return false;
@@ -171,7 +171,7 @@
}
// set initial position, twist
- FrameVel frame_twist;
+ FrameVel frame_twist;
chain_.getVelocities(robot_state_->joint_states_, jnt_posvel_);
jnt_to_twist_solver_->JntToCart(jnt_posvel_, frame_twist);
pose_meas_old_ = frame_twist.value();
@@ -193,7 +193,7 @@
chain_.getVelocities(robot_state_->joint_states_, jnt_posvel_);
// get cartesian twist and pose
- FrameVel frame_twist;
+ FrameVel frame_twist;
jnt_to_twist_solver_->JntToCart(jnt_posvel_, frame_twist);
pose_meas_ = frame_twist.value();
twist_meas_ = pose_meas_.M.Inverse() * (frame_twist.deriv());
@@ -212,7 +212,7 @@
else if (mode_[i] == tff_msg_.POSITION)
wrench_desi_[i] = twist_to_wrench_[i] * (pos_pid_controller_[i].updatePid(position_[i] - value_[i], dt));
}
-
+
// send wrench to wrench controller
wrench_controller_->wrench_desi_ = (pose_meas_.M * wrench_desi_);
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -34,8 +34,8 @@
#include <algorithm>
-#include <robot_kinematics/robot_kinematics.h>
#include <mechanism_control/mechanism_control.h>
+#include "kdl/chainfksolverpos_recursive.hpp"
#include "robot_mechanism_controllers/cartesian_trajectory_controller.h"
@@ -116,7 +116,7 @@
ROS_ERROR("CartesianTrajectoryController: could not get instance to mechanism control");
return false;
}
- string output;
+ std::string output;
if (!node_->getParam(controller_name_+"/output", output)){
ROS_ERROR("CartesianTrajectoryController: No ouptut name found on parameter server");
return false;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -32,8 +32,8 @@
*/
#include <algorithm>
-#include <robot_kinematics/robot_kinematics.h>
#include <mechanism_control/mechanism_control.h>
+#include "kdl/chainfksolvervel_recursive.hpp"
#include "robot_mechanism_controllers/cartesian_twist_controller.h"
using namespace KDL;
@@ -89,7 +89,7 @@
chain_.toKDL(kdl_chain_);
// create solver
- jnt_to_twist_solver_.reset(new ChainFkSolverVel_recursive(kdl_chain_));
+ jnt_to_twist_solver_.reset(new KDL::ChainFkSolverVel_recursive(kdl_chain_));
jnt_posvel_.resize(kdl_chain_.getNrOfJoints());
// constructs 3 identical pid controllers: for the x,y and z translations
@@ -113,7 +113,7 @@
ROS_ERROR("CartesianTwistController: could not get instance to mechanism control");
return false;
}
- string output;
+ std::string output;
if (!node_->getParam(controller_name_+"/output", output)){
ROS_ERROR("CartesianTwistController: No ouptut name found on parameter server");
return false;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller_ik.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller_ik.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller_ik.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -31,9 +31,9 @@
* Author: Wim Meeussen
*/
+#include "robot_mechanism_controllers/cartesian_twist_controller_ik.h"
#include <algorithm>
-#include <robot_kinematics/robot_kinematics.h>
-#include "robot_mechanism_controllers/cartesian_twist_controller_ik.h"
+#include <kdl/chainfksolvervel_recursive.hpp>
using namespace KDL;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -32,7 +32,6 @@
*/
#include <algorithm>
-#include <robot_kinematics/robot_kinematics.h>
#include <robot_mechanism_controllers/cartesian_wrench_controller.h>
@@ -156,7 +155,7 @@
-bool CartesianWrenchController::publishDiagnostics(int level, const string& message)
+bool CartesianWrenchController::publishDiagnostics(int level, const std::string& message)
{
if (diagnostics_time_ + diagnostics_interval_ < ros::Time::now())
{
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -30,10 +30,13 @@
/*
* Author: Melonee Wise
*/
+
+#include "robot_mechanism_controllers/endeffector_constraint_controller.h"
+#include <algorithm>
#include "angles/angles.h"
#include "urdf/parser.h"
-#include <algorithm>
-#include "robot_mechanism_controllers/endeffector_constraint_controller.h"
+#include "kdl/chainfksolverpos_recursive.hpp"
+#include "kdl/chainjnttojacsolver.hpp"
#define DEBUG 0 // easy debugging
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_chain_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_chain_constraint_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_chain_constraint_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -30,11 +30,11 @@
/*
* Author: Melonee Wise
*/
-
+
+#include "robot_mechanism_controllers/joint_chain_constraint_controller.h"
+#include <algorithm>
#include "angles/angles.h"
-#include <algorithm>
-#include "robot_kinematics/robot_kinematics.h"
-#include "robot_mechanism_controllers/joint_chain_constraint_controller.h"
+#include <kdl/chainfksolvervel_recursive.hpp>
using namespace KDL;
@@ -58,10 +58,10 @@
}
-bool JointChainConstraintController::init(mechanism::RobotState *robot_state,
- const string& root_name,
- const string& tip_name,
- const string& controller_name)
+bool JointChainConstraintController::init(mechanism::RobotState *robot_state,
+ const std::string& root_name,
+ const std::string& tip_name,
+ const std::string& controller_name)
{
ROS_INFO("Intializing JointChainConstraintController,\"%s\", between \"%s\" and \"%s\".", controller_name.c_str(), root_name.c_str(), tip_name.c_str());
@@ -84,9 +84,9 @@
jnt_pos_.resize(kdl_chain_.getNrOfJoints());
jnt_eff_.resize(kdl_chain_.getNrOfJoints());
chain_kdl_jacobian_.resize(kdl_chain_.getNrOfJoints());
-
+
wrench_desired_ = Wrench::Zero();
-
+
// create the required matrices
joint_constraint_torque_ = Eigen::MatrixXf::Zero(kdl_chain_.getNrOfJoints(), 1);
// TODO: make sure to change this in the future to have mixed constraint conditions on joints
@@ -99,7 +99,7 @@
// advertise service
node_->advertiseService(controller_name_ + "/add_constraints", &JointChainConstraintController::addConstraint, this);
- change_constraints_guard_.set(controller_name_ + "/add_constraints");
+ change_constraints_guard_.set(controller_name_ + "/add_constraints");
return true;
}
@@ -111,13 +111,13 @@
// check if joints are calibrated
if (!mechanism_chain_.allCalibrated(robot_state_->joint_states_))
return;
-
+
// get joint positions
mechanism_chain_.getPositions(robot_state_->joint_states_, jnt_pos_);
-
+
// get the chain jacobian
jnt_to_jac_solver_->JntToJac(jnt_pos_, chain_kdl_jacobian_);
-
+
// TODO: Write a function for doing this conversion
//convert to eigen for easier math
for (unsigned int i = 0; i < 6; i++)
@@ -127,21 +127,21 @@
chain_eigen_jacobian_(i,j) = chain_kdl_jacobian_(i,j);
}
}
-
+
// compute constraint torques
computeConstraintTorques();
-
+
// compute constraint torques
computeConstraintJacobian();
-
+
// compute constraint torques
computeConstraintNullSpace();
-
- // compute the task torque on the joints
+
+ // compute the task torque on the joints
for (unsigned int i=0; i<6; i++)
task_wrench_(i) = wrench_desired_(i);
task_torque_ = joint_constraint_null_space_ * chain_eigen_jacobian_.transpose() * task_wrench_;
-
+
for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); ++i)
{
jnt_eff_(i) = joint_constraint_torque_(i) + task_torque_(i);
@@ -158,23 +158,23 @@
joint_constraint_torque_.setZero();
// check there are constraints to enforce
if(list_size_ == 0)
- {
+ {
return;
}
-
+
double time = robot_state_->hw_->current_time_;
double error(0);
std::list<ConstraintState>::iterator it = constraint_list_.begin();
for(int i = 0 ; i < list_size_; ++i)
{
-
+
int joint_type = (*it).joint_->type_;
-
+
if(joint_type == mechanism::JOINT_ROTARY)
{
double limit_min = (*it).joint_->joint_limit_min_;
double limit_max = (*it).joint_->joint_limit_max_;
-
+
angles::shortest_angular_distance_with_limits((*it).threshold_start_, jnt_pos_((*it).joint_chain_index_), limit_min, limit_max, error);
}
@@ -189,9 +189,9 @@
// check the sign of the constraint
double sign = -((*it).threshold_start_ - (*it).nullspace_start_)/fabs((*it).threshold_start_ - (*it).nullspace_start_);
-
+
(*it).joint_error_ = error;
-
+
if(sign*error > 0)
{
double temp_constraint_torque = (*it).pid_.updatePid(error, time - last_time_);
@@ -201,7 +201,7 @@
// sum the torques
joint_constraint_torque_((*it).joint_chain_index_) = joint_constraint_torque_((*it).joint_chain_index_)+ temp_constraint_torque;
}
- else
+ else
{
// make error 0 for better accounting
(*it).joint_error_ = 0;
@@ -209,7 +209,7 @@
it++;
}
last_time_ = time;
-
+
}
@@ -219,11 +219,11 @@
joint_constraint_jacobian_.setZero();
// check there are constraints to enforce
if(list_size_ == 0)
- {
+ {
return;
}
std::list<ConstraintState>::iterator it = constraint_list_.begin();
-
+
for(int i = 0 ; i < list_size_; ++i)
{
// check to turn off control authority
@@ -233,7 +233,7 @@
}
it++;
}
-
+
}
@@ -247,12 +247,12 @@
}
-bool JointChainConstraintController::addConstraint(robot_mechanism_controllers::ChangeConstraints::Request &req,
+bool JointChainConstraintController::addConstraint(robot_mechanism_controllers::ChangeConstraints::Request &req,
robot_mechanism_controllers::ChangeConstraints::Response &resp)
{
ConstraintState temp;
bool found = false;
-
+
for(unsigned int i =0; i < kdl_chain_.getNrOfJoints(); i++)
{
std::string name = mechanism_chain_.getJoint(i)->name_;
@@ -265,7 +265,7 @@
break;
}
}
-
+
if (!found) {
ROS_ERROR("Unable to find joint with ID: \"%s\"", req.constraint.joint_name.c_str());
resp.add_ok = 0;
@@ -273,7 +273,7 @@
}
temp.pid_.initPid(req.constraint.p,req.constraint.i,req.constraint.d,req.constraint.i_clamp, -req.constraint.i_clamp);
-
+
temp.id_ = req.constraint.id;
temp.joint_name_ = req.constraint.joint_name;
temp.threshold_start_ = req.constraint.threshold_start;
@@ -281,11 +281,11 @@
temp.max_constraint_torque_ = req.constraint.max_constraint_torque;
temp.remove_ = false;
temp.joint_error_ = 0;
-
+
constraint_list_.push_back(temp);
list_size_++;
ROS_INFO("Added constraint on \"%s\" with ID: \"%s\".", req.constraint.joint_name.c_str(),req.constraint.id.c_str());
-
+
resp.add_ok = 1;
return true;
@@ -316,10 +316,10 @@
controller_name_ = config->Attribute("name");
// get name of root and tip
- string root_name, tip_name;
- node_->param(controller_name_+"/root_name", root_name, string("no_name_given"));
- node_->param(controller_name_+"/tip_name", tip_name, string("no_name_given"));
-
+ std::string root_name, tip_name;
+ node_->param(controller_name_+"/root_name", root_name, std::string("no_name_given"));
+ node_->param(controller_name_+"/tip_name", tip_name, std::string("no_name_given"));
+
// initialize wrench controller
if (!controller_.init(robot, root_name, tip_name, controller_name_))
{
@@ -330,8 +330,8 @@
// subscribe to wrench commands
node_->subscribe(controller_name_ + "/command", wrench_msg_,
&JointChainConstraintControllerNode::command, this, 1);
-
+
return true;
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -30,11 +30,13 @@
/*
* Author: Melonee Wise
*/
+#include "robot_mechanism_controllers/plug_controller.h"
+#include <algorithm>
#include "angles/angles.h"
#include "urdf/parser.h"
-#include <algorithm>
-#include "robot_mechanism_controllers/plug_controller.h"
#include "tf_conversions/tf_kdl.h"
+#include "kdl/chainfksolverpos_recursive.hpp"
+#include "kdl/chainjnttojacsolver.hpp"
#define DEBUG 0 // easy debugging
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2009-04-23 20:09:28 UTC (rev 14346)
@@ -91,11 +91,11 @@
// get controller
controller::Controller* controller = getControllerByName(name);
if (controller == NULL) return false;
-
+
// cast controller to ControllerType
ControllerType* controller_type = dynamic_cast< ControllerType* >(controller);
if (controller_type == NULL) return false;
-
+
// copy result
c = controller_type;
return true;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
