Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3954] pkg/trunk
|
From: <stu...@us...> - 2008-09-04 21:54:11
|
Revision: 3954
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3954&view=rev
Author: stuglaser
Date: 2008-09-04 21:54:19 +0000 (Thu, 04 Sep 2008)
Log Message:
-----------
Separated robot state from the robot model and ported all the controllers.
Modified Paths:
--------------
pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h
pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_effort_controller.h
pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_pd_controller.h
pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h
pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h
pkg/trunk/controllers/generic_controllers/include/generic_controllers/ramp_effort_controller.h
pkg/trunk/controllers/generic_controllers/include/generic_controllers/sine_sweep_controller.h
pkg/trunk/controllers/generic_controllers/src/joint_effort_controller.cpp
pkg/trunk/controllers/generic_controllers/src/joint_pd_controller.cpp
pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp
pkg/trunk/controllers/generic_controllers/src/joint_velocity_controller.cpp
pkg/trunk/controllers/generic_controllers/src/ramp_effort_controller.cpp
pkg/trunk/controllers/generic_controllers/src/sine_sweep_controller.cpp
pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/arm_position_controller.h
pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/arm_velocity_controller.h
pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h
pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/laser_scanner_controller.h
pkg/trunk/controllers/pr2_controllers/src/arm_position_controller.cpp
pkg/trunk/controllers/pr2_controllers/src/arm_velocity_controller.cpp
pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
pkg/trunk/controllers/pr2_controllers/src/laser_scanner_controller.cpp
pkg/trunk/controllers/pr2_controllers/test/test_base_controller.cpp
pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test1.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/simple_transmission.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
pkg/trunk/mechanism/mechanism_model/src/chain.cpp
pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
pkg/trunk/mechanism/mechanism_model/src/joint.cpp
pkg/trunk/mechanism/mechanism_model/src/link.cpp
pkg/trunk/mechanism/mechanism_model/src/robot.cpp
pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h 2008-09-04 21:54:19 UTC (rev 3954)
@@ -46,7 +46,6 @@
#include <mechanism_model/robot.h>
#include <tinyxml/tinyxml.h>
-//class TiXmlElement;
namespace controller
{
@@ -89,7 +88,7 @@
{
}
virtual void update(void) = 0;
- virtual bool initXml(mechanism::Robot *robot, TiXmlElement *config) = 0;
+ virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config) = 0;
};
}
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_effort_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_effort_controller.h 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_effort_controller.h 2008-09-04 21:54:19 UTC (rev 3954)
@@ -77,8 +77,7 @@
* \brief Functional way to initialize limits and gains.
*
*/
- void init(std::string name,mechanism::Robot *robot);
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
/*!
* \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
@@ -109,8 +108,8 @@
virtual void update();
private:
- mechanism::Joint* joint_; /**< Joint we're controlling. */
- mechanism::Robot *robot_; /**< Pointer to robot structure. */
+ mechanism::JointState *joint_; /**< Joint we're controlling. */
+ mechanism::RobotState *robot_; /**< Pointer to robot structure. */
double command_; /**< Last commanded position. */
};
@@ -140,8 +139,7 @@
void update();
- void init(std::string name, mechanism::Robot *robot);
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
// Services
bool setCommand(generic_controllers::SetCommand::request &req,
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_pd_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_pd_controller.h 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_pd_controller.h 2008-09-04 21:54:19 UTC (rev 3954)
@@ -44,9 +44,9 @@
Example config:
<controller type="JointPDController" name="controller_name">
- <joint name="joint_to_control">
- <pid p="1.0" i="2.0" d="3.0" iClamp="4.0" />
- </joint>
+ <joint name="joint_to_control">
+ <pid p="1.0" i="2.0" d="3.0" iClamp="4.0" />
+ </joint>
</controller>
*/
/***************************************************/
@@ -88,8 +88,8 @@
* \param *joint The joint that is being controlled.
*/
- void init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::Robot *robot);
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ void init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::RobotState *robot);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
/*!
* \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
@@ -130,22 +130,15 @@
std::string getJointName();
- private:
+ private:
- mechanism::Joint* joint_; /**< Joint we're controlling. */
-
- mechanism::Robot *robot_; /**< Pointer to robot structure. */
-
+ mechanism::JointState* joint_; /**< Joint we're controlling. */
+ mechanism::RobotState *robot_; /**< Pointer to robot structure. */
Pid pid_controller_; /**< Internal PID controller. */
-
double last_time_; /**< Last time stamp of update. */
-
double command_; /**< Last commanded position. */
-
double command_dot_;
-
double command_t_; /**< Last commanded position. */
-
double command_dot_t_;
/*!
@@ -185,8 +178,8 @@
void update();
- void init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::Robot *robot);
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ void init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::RobotState *robot);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
// Services
bool setPDCommand(generic_controllers::SetPDCommand::request &req,
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h 2008-09-04 21:54:19 UTC (rev 3954)
@@ -87,8 +87,7 @@
* \param time The current hardware time.
* \param *joint The joint that is being controlled.
*/
- void init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::Robot *robot);
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
/*!
* \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
@@ -125,8 +124,8 @@
std::string getJointName();
private:
- mechanism::Joint* joint_; /**< Joint we're controlling. */
- mechanism::Robot *robot_; /**< Pointer to robot structure. */
+ mechanism::JointState *joint_state_; /**< Joint we're controlling. */
+ mechanism::RobotState *robot_; /**< Pointer to robot structure. */
Pid pid_controller_; /**< Internal PID controller. */
double last_time_; /**< Last time stamp of update. */
double command_; /**< Last commanded position. */
@@ -160,8 +159,7 @@
void update();
- void init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::Robot *robot);
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
// Services
bool setCommand(generic_controllers::SetCommand::request &req,
@@ -173,7 +171,7 @@
double getMeasuredPosition();
bool getActual(generic_controllers::GetActual::request &req,
- generic_controllers::GetActual::response &resp);
+ generic_controllers::GetActual::response &resp);
private:
JointPositionController *c_;
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h 2008-09-04 21:54:19 UTC (rev 3954)
@@ -88,7 +88,7 @@
*/
void init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::Robot *robot);
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
/*!
* \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
@@ -125,8 +125,8 @@
std::string getJointName();
private:
- mechanism::Joint* joint_; /**< Joint we're controlling. */
- mechanism::Robot *robot_; /**< Pointer to robot structure. */
+ mechanism::JointState *joint_state_;
+ mechanism::RobotState *robot_; /**< Pointer to robot structure. */
Pid pid_controller_; /**< Internal PID controller. */
double last_time_; /**< Last time stamp of update. */
double command_; /**< Last commanded position. */
@@ -163,8 +163,7 @@
void update();
- void init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::Robot *robot);
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
// Services
bool setCommand(generic_controllers::SetCommand::request &req,
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/ramp_effort_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/ramp_effort_controller.h 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/ramp_effort_controller.h 2008-09-04 21:54:19 UTC (rev 3954)
@@ -69,18 +69,9 @@
*/
~RampEffortController();
- /*!
- * \brief Functional way to initialize.
- * \param input_start The start value of the ramp.
- * \param input_end The end value of the ramp.
- * \param duration The duration in seconds from start to finish.
- * \param time The current hardware time.
- * \param *joint The joint that is being controlled.
- */
- void init(double input_start, double input_end, double duration, double time,std::string name,mechanism::Robot *robot);
+ void init(double input_start, double input_end, double duration, double time,std::string name,mechanism::RobotState *robot);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
-
/*!
* \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
*/
@@ -108,8 +99,8 @@
virtual void update();
private:
- mechanism::Joint* joint_; /**< Joint we're controlling. */
- mechanism::Robot *robot_; /**< Pointer to robot structure. */
+ mechanism::JointState *joint_state_; /**< Joint we're controlling. */
+ mechanism::RobotState *robot_; /**< Pointer to robot structure. */
double input_start_; /**< Begining of the ramp. */
double input_end_; /**< End of the ramp. */
double duration_; /**< Duration of the ramp. */
@@ -143,8 +134,7 @@
void update();
- void init(double input_start, double input_end, double duration, double time,std::string name,mechanism::Robot *robot);
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
bool getActual(generic_controllers::GetActual::request &req,
generic_controllers::GetActual::response &resp);
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/sine_sweep_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/sine_sweep_controller.h 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/sine_sweep_controller.h 2008-09-04 21:54:19 UTC (rev 3954)
@@ -79,9 +79,9 @@
* \param time The current hardware time.
* \param *robot The robot that is being controlled.
*/
- void init(double start_freq, double end_freq, double duration, double amplitude, double time,std::string name,mechanism::Robot *robot);
+ void init(double start_freq, double end_freq, double duration, double amplitude, double time,std::string name,mechanism::RobotState *robot);
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
/*!
* \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
@@ -106,8 +106,8 @@
virtual void update();
private:
- mechanism::Joint* joint_; /**< Joint we're controlling. */
- mechanism::Robot *robot_; /**< Pointer to robot structure. */
+ mechanism::JointState *joint_state_; /**< Joint we're controlling. */
+ mechanism::RobotState *robot_; /**< Pointer to robot structure. */
double start_freq_; /**< Begining of the sweep. */
double end_freq_; /**< End of the sweep. */
double amplitude_; /**< Amplitude of the sweep. */
@@ -146,8 +146,7 @@
void update();
- void init(double start_freq, double end_freq, double duration, double amplitude, double time,std::string name,mechanism::Robot *robot);
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
private:
SineSweepController *c_;
Modified: pkg/trunk/controllers/generic_controllers/src/joint_effort_controller.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/joint_effort_controller.cpp 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/generic_controllers/src/joint_effort_controller.cpp 2008-09-04 21:54:19 UTC (rev 3954)
@@ -50,16 +50,8 @@
{
}
-void JointEffortController::init(std::string name,mechanism::Robot *robot)
+bool JointEffortController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
- robot_ = robot;
- joint_ = robot->getJoint(name);
-
- command_= 0;
-}
-
-bool JointEffortController::initXml(mechanism::Robot *robot, TiXmlElement *config)
-{
assert(robot);
robot_ = robot;
@@ -71,7 +63,7 @@
}
const char *joint_name = j->Attribute("name");
- joint_ = joint_name ? robot->getJoint(joint_name) : NULL;
+ joint_ = joint_name ? robot->getJointState(joint_name) : NULL;
if (!joint_)
{
fprintf(stderr, "JointVelocityController could not find joint named \"%s\"\n", joint_name);
@@ -145,21 +137,10 @@
return true;
}
-void JointEffortControllerNode::init(std::string name, mechanism::Robot *robot)
+bool JointEffortControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
ros::node *node = ros::node::instance();
- string prefix = name;
- c_->init(name, robot);
- node->advertise_service(prefix + "/set_command", &JointEffortControllerNode::setCommand, this);
- node->advertise_service(prefix + "/get_actual", &JointEffortControllerNode::getActual, this);
-}
-
-
-bool JointEffortControllerNode::initXml(mechanism::Robot *robot, TiXmlElement *config)
-{
- ros::node *node = ros::node::instance();
-
std::string topic = config->Attribute("topic") ? config->Attribute("topic") : "";
if (topic == "")
{
Modified: pkg/trunk/controllers/generic_controllers/src/joint_pd_controller.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/joint_pd_controller.cpp 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/generic_controllers/src/joint_pd_controller.cpp 2008-09-04 21:54:19 UTC (rev 3954)
@@ -57,10 +57,10 @@
{
}
-void JointPDController::init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::Robot *robot)
+void JointPDController::init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::RobotState *robot)
{
robot_ = robot;
- joint_ = robot->getJoint(name);
+ joint_ = robot->getJointState(name);
pid_controller_.initPid(p_gain, i_gain, d_gain, windup, -windup);
command_= 0;
@@ -69,7 +69,7 @@
}
-bool JointPDController::initXml(mechanism::Robot *robot, TiXmlElement *config)
+bool JointPDController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
assert(robot);
robot_ = robot;
@@ -83,7 +83,7 @@
}
const char *joint_name = j->Attribute("name");
- joint_ = joint_name ? robot->getJoint(joint_name) : NULL;
+ joint_ = joint_name ? robot->getJointState(joint_name) : NULL;
if (!joint_)
{
fprintf(stderr, "JointPDController could not find joint named \"%s\"\n", joint_name);
@@ -137,7 +137,7 @@
std::string JointPDController::getJointName()
{
- return(joint_->name_);
+ return(joint_->joint_->name_);
}
void JointPDController::update()
@@ -218,7 +218,7 @@
}
-void JointPDControllerNode::init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::Robot *robot)
+void JointPDControllerNode::init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::RobotState *robot)
{
ros::node *node = ros::node::instance();
string prefix = name;
@@ -228,7 +228,7 @@
node->advertise_service(prefix + "/get_actual", &JointPDControllerNode::getPDActual, this);
}
-bool JointPDControllerNode::initXml(mechanism::Robot *robot, TiXmlElement *config)
+bool JointPDControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
ros::node *node = ros::node::instance();
Modified: pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp 2008-09-04 21:54:19 UTC (rev 3954)
@@ -40,10 +40,8 @@
ROS_REGISTER_CONTROLLER(JointPositionController)
JointPositionController::JointPositionController()
+: joint_state_(NULL), robot_(NULL)
{
- robot_ = NULL;
- joint_ = NULL;
-
// Initialize PID class
pid_controller_.initPid(0, 0, 0, 0, 0);
command_ = 0;
@@ -54,18 +52,8 @@
{
}
-void JointPositionController::init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::Robot *robot)
+bool JointPositionController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
- robot_ = robot;
- joint_ = robot->getJoint(name);
-
- pid_controller_.initPid(p_gain, i_gain, d_gain, windup, -windup);
- command_= 0;
- last_time_= time;
-}
-
-bool JointPositionController::initXml(mechanism::Robot *robot, TiXmlElement *config)
-{
assert(robot);
robot_ = robot;
last_time_ = robot->hw_->current_time_;
@@ -78,12 +66,13 @@
}
const char *joint_name = j->Attribute("name");
- joint_ = joint_name ? robot->getJoint(joint_name) : NULL;
- if (!joint_)
+ int index = joint_name ? robot->model_->getJointIndex(joint_name) : -1;
+ if (index < 0)
{
fprintf(stderr, "JointPositionController could not find joint named \"%s\"\n", joint_name);
return false;
}
+ joint_state_ = &robot->joint_states_[index];
TiXmlElement *p = j->FirstChildElement("pid");
if (p)
@@ -112,7 +101,7 @@
std::string JointPositionController::getJointName()
{
- return(joint_->name_);
+ return joint_state_->joint_->name_;
}
// Return the current position command
@@ -124,7 +113,7 @@
// Return the measured joint position
double JointPositionController::getMeasuredPosition()
{
- return joint_->position_;
+ return joint_state_->position_;
}
double JointPositionController::getTime()
@@ -138,18 +127,19 @@
double error(0);
double time = robot_->hw_->current_time_;
- if(joint_)
+ if(joint_state_->joint_)
{
- if(joint_->type_ == mechanism::JOINT_ROTARY || joint_->type_ == mechanism::JOINT_CONTINUOUS)
+ if(joint_state_->joint_->type_ == mechanism::JOINT_ROTARY ||
+ joint_state_->joint_->type_ == mechanism::JOINT_CONTINUOUS)
{
- error = math_utils::shortest_angular_distance(command_, joint_->position_);
+ error = math_utils::shortest_angular_distance(command_, joint_state_->position_);
}
else
{
- error = joint_->position_ - command_;
+ error = joint_state_->position_ - command_;
}
- joint_->commanded_effort_ = pid_controller_.updatePid(error, time - last_time_);
+ joint_state_->commanded_effort_ = pid_controller_.updatePid(error, time - last_time_);
}
last_time_ = time;
}
@@ -207,19 +197,10 @@
return c_->getMeasuredPosition();
}
-void JointPositionControllerNode::init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::Robot *robot)
+bool JointPositionControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
ros::node *node = ros::node::instance();
- string prefix = name;
-
- c_->init(p_gain, i_gain, d_gain, windup, time, name,robot);
- node->advertise_service(prefix + "/set_command", &JointPositionControllerNode::setCommand, this);
- node->advertise_service(prefix + "/get_actual", &JointPositionControllerNode::getActual, this);
-}
-
-bool JointPositionControllerNode::initXml(mechanism::Robot *robot, TiXmlElement *config)
-{
- ros::node *node = ros::node::instance();
+ assert(node);
string prefix = config->Attribute("name");
std::string topic = config->Attribute("topic") ? config->Attribute("topic") : "";
Modified: pkg/trunk/controllers/generic_controllers/src/joint_velocity_controller.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/joint_velocity_controller.cpp 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/generic_controllers/src/joint_velocity_controller.cpp 2008-09-04 21:54:19 UTC (rev 3954)
@@ -41,10 +41,8 @@
ROS_REGISTER_CONTROLLER(JointVelocityController)
JointVelocityController::JointVelocityController()
+: joint_state_(NULL), robot_(NULL)
{
- robot_=NULL;
- joint_=NULL;
-
// Initialize PID class
pid_controller_.initPid(0, 0, 0, 0, 0);
command_ = 0;
@@ -55,19 +53,8 @@
{
}
-void JointVelocityController::init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::Robot *robot)
+bool JointVelocityController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
- robot_ = robot;
- joint_ = robot->getJoint(name);
-
- pid_controller_.initPid(p_gain, i_gain, d_gain, windup, -windup);
- command_= 0;
- last_time_= time;
-
-}
-
-bool JointVelocityController::initXml(mechanism::Robot *robot, TiXmlElement *config)
-{
assert(robot);
robot_ = robot;
last_time_ = robot->hw_->current_time_;
@@ -80,12 +67,13 @@
}
const char *joint_name = j->Attribute("name");
- joint_ = joint_name ? robot->getJoint(joint_name) : NULL;
- if (!joint_)
+ int index = joint_name ? robot->model_->getJointIndex(joint_name) : NULL;
+ if (index < 0)
{
fprintf(stderr, "JointVelocityController could not find joint named \"%s\"\n", joint_name);
return false;
}
+ joint_state_ = &robot_->joint_states_[index];
TiXmlElement *p = j->FirstChildElement("pid");
if (p)
@@ -116,12 +104,12 @@
// Return the measured joint velocity
double JointVelocityController::getMeasuredVelocity()
{
- return joint_->velocity_;
+ return joint_state_->velocity_;
}
std::string JointVelocityController::getJointName()
{
- return(joint_->name_);
+ return joint_state_->joint_->name_;
}
void JointVelocityController::update()
@@ -129,8 +117,8 @@
double error(0);
double time = robot_->hw_->current_time_;
- error = joint_->velocity_ - command_;
- joint_->commanded_effort_ = pid_controller_.updatePid(error, time - last_time_);
+ error = joint_state_->velocity_ - command_;
+ joint_state_->commanded_effort_ = pid_controller_.updatePid(error, time - last_time_);
last_time_ = time;
}
@@ -181,20 +169,10 @@
return true;
}
-void JointVelocityControllerNode::init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::Robot *robot)
+bool JointVelocityControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
ros::node *node = ros::node::instance();
- string prefix = name;
- c_->init(p_gain, i_gain, d_gain, windup, time,name, robot);
- node->advertise_service(prefix + "/set_command", &JointVelocityControllerNode::setCommand, this);
- node->advertise_service(prefix + "/get_actual", &JointVelocityControllerNode::getActual, this);
-}
-
-bool JointVelocityControllerNode::initXml(mechanism::Robot *robot, TiXmlElement *config)
-{
- ros::node *node = ros::node::instance();
-
std::string topic = config->Attribute("topic") ? config->Attribute("topic") : "";
if (topic == "")
{
Modified: pkg/trunk/controllers/generic_controllers/src/ramp_effort_controller.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/ramp_effort_controller.cpp 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/generic_controllers/src/ramp_effort_controller.cpp 2008-09-04 21:54:19 UTC (rev 3954)
@@ -34,15 +34,14 @@
#include <generic_controllers/ramp_effort_controller.h>
using namespace std;
-using namespace controller;
+namespace controller {
+
ROS_REGISTER_CONTROLLER(RampEffortController)
RampEffortController::RampEffortController()
+: joint_state_(NULL), robot_(NULL)
{
- robot_ = NULL;
- joint_ = NULL;
-
input_start_ = 0;
input_end_ = 0;
duration_ = 0;
@@ -53,21 +52,21 @@
{
}
-void RampEffortController::init(double input_start, double input_end, double duration, double time,std::string name,mechanism::Robot *robot)
+void RampEffortController::init(double input_start, double input_end, double duration, double time,std::string name,mechanism::RobotState *robot)
{
robot_ = robot;
- joint_ = robot->getJoint(name);
-
-
+ int index = robot->model_->getJointIndex(name);
+ joint_state_ = &robot->joint_states_[index];
+
input_start_=input_start;
input_end_=input_end;
duration_=duration;
initial_time_=time;
}
-bool RampEffortController::initXml(mechanism::Robot *robot, TiXmlElement *config)
+bool RampEffortController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
-
+
TiXmlElement *j = config->FirstChildElement("joint");
if (!j)
{
@@ -76,13 +75,13 @@
}
const char *joint_name = j->Attribute("name");
- joint_ = joint_name ? robot->getJoint(joint_name) : NULL;
- if (!joint_)
+ joint_state_ = joint_name ? robot->getJointState(joint_name) : NULL;
+ if (!joint_state_)
{
fprintf(stderr, "RampEffortController could not find joint named \"%s\"\n", joint_name);
return false;
}
-
+
TiXmlElement *cd = j->FirstChildElement("controller_defaults");
if (cd)
{
@@ -99,19 +98,19 @@
// Return the current position command
double RampEffortController::getCommand()
{
- return joint_->commanded_effort_;
+ return joint_state_->commanded_effort_;
}
// Return the measured joint position
double RampEffortController::getMeasuredEffort()
{
- return joint_->applied_effort_;
+ return joint_state_->applied_effort_;
}
// Return the measured joint position
double RampEffortController::getVelocity()
{
- return joint_->velocity_;
+ return joint_state_->velocity_;
}
double RampEffortController::getTime()
@@ -123,7 +122,7 @@
{
double time = robot_->hw_->current_time_;
- joint_->commanded_effort_ = input_start_+(input_end_-input_start_)*(time-initial_time_)/(duration_);
+ joint_state_->commanded_effort_ = input_start_+(input_end_-input_start_)*(time-initial_time_)/(duration_);
}
ROS_REGISTER_CONTROLLER(RampEffortControllerNode)
@@ -153,13 +152,8 @@
return true;
}
-void RampEffortControllerNode::init(double input_start, double input_end, double duration, double time,std::string name,mechanism::Robot *robot)
+bool RampEffortControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
- assert(false); // temporary fix for lack of xml
-}
-
-bool RampEffortControllerNode::initXml(mechanism::Robot *robot, TiXmlElement *config)
-{
ros::node *node = ros::node::instance();
string prefix = config->Attribute("name");
@@ -169,3 +163,4 @@
return true;
}
+}
Modified: pkg/trunk/controllers/generic_controllers/src/sine_sweep_controller.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/sine_sweep_controller.cpp 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/generic_controllers/src/sine_sweep_controller.cpp 2008-09-04 21:54:19 UTC (rev 3954)
@@ -35,14 +35,14 @@
#include <math.h>
using namespace std;
-using namespace controller;
+namespace controller {
ROS_REGISTER_CONTROLLER(SineSweepController)
SineSweepController::SineSweepController()
{
robot_ = NULL;
- joint_ = NULL;
+ joint_state_ = NULL;
start_freq_=0;
end_freq_=0;
@@ -55,24 +55,24 @@
{
}
-void SineSweepController::init(double start_freq, double end_freq, double duration, double amplitude, double time,std::string name,mechanism::Robot *robot)
+void SineSweepController::init(double start_freq, double end_freq, double duration, double amplitude, double time,std::string name,mechanism::RobotState *robot)
{
robot_ = robot;
- joint_ = robot->getJoint(name);
+ joint_state_ = robot->getJointState(name);
start_freq_=start_freq; //in Hz
end_freq_=end_freq; //in Hz
amplitude_=amplitude; //in Newtons
duration_=duration; //in seconds
initial_time_=time;
-
+
start_angular_freq_ =2*M_PI*start_freq_;
end_angular_freq_ =2*M_PI*end_freq_;
K_factor_ = (start_angular_freq_*duration_)/log(end_angular_freq_/start_angular_freq_);
L_factor_ = (duration_)/log(end_angular_freq_/start_angular_freq_);
}
-bool SineSweepController::initXml(mechanism::Robot *robot, TiXmlElement *config)
+bool SineSweepController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
TiXmlElement *jnt = config->FirstChildElement("joint");
@@ -90,13 +90,13 @@
// Return the current position command
double SineSweepController::getCommand()
{
- return joint_->commanded_effort_;
+ return joint_state_->commanded_effort_;
}
// Return the measured joint position
double SineSweepController::getMeasuredEffort()
{
- return joint_->applied_effort_;
+ return joint_state_->applied_effort_;
}
@@ -108,10 +108,10 @@
void SineSweepController::update()
{
double time = robot_->hw_->current_time_;
-
+
if((time-initial_time_)<duration_)
{
- joint_->commanded_effort_ = amplitude_*sin(K_factor_*(exp((time-initial_time_)/(L_factor_))-1));
+ joint_state_->commanded_effort_ = amplitude_*sin(K_factor_*(exp((time-initial_time_)/(L_factor_))-1));
}
}
@@ -132,13 +132,8 @@
}
-void SineSweepControllerNode::init(double start_freq, double end_freq, double duration, double amplitude, double time,std::string name,mechanism::Robot *robot)
+bool SineSweepControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
- assert(false); // temporary fix for lack of xml
-}
-
-bool SineSweepControllerNode::initXml(mechanism::Robot *robot, TiXmlElement *config)
-{
string prefix = config->Attribute("name");
c_->initXml(robot, config);
@@ -146,3 +141,4 @@
return true;
}
+}
\ No newline at end of file
Modified: pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/arm_position_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/arm_position_controller.h 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/arm_position_controller.h 2008-09-04 21:54:19 UTC (rev 3954)
@@ -63,11 +63,11 @@
// The maximum number of joints expected in an arm.
static const int MAX_ARM_JOINTS = 7;
-
+
class ArmPositionController : public Controller
{
public:
-
+
/*!
* \brief Default Constructor of the JointController class.
*
@@ -83,8 +83,8 @@
* \brief Functional way to initialize limits and gains.
*
*/
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
-
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
/*!
* \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
*
@@ -103,7 +103,7 @@
* \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
*/
void getJointPosCmd(pr2_controllers::GetJointPosCmd::response &resp);
-
+
void getCurrentConfiguration(std::vector<double> &);
/*!
* \brief Issues commands to the joint. Should be called at regular intervals
@@ -120,14 +120,14 @@
private:
- std::vector<JointPositionController *> joint_position_controllers_;
+ std::vector<JointPositionController *> joint_position_controllers_;
// Goal of the joints
std::vector<double> goals_;
// Goal of the joints - used by the realtime code only
- std::vector<double> goals_rt_;
+ std::vector<double> goals_rt_;
- mechanism::Robot* robot_;
+ mechanism::RobotState* robot_;
void updateJointControllers(void);
@@ -149,7 +149,7 @@
void update();
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
void setJointPosCmd(std::vector<double> &req_goals_);
@@ -169,7 +169,7 @@
* \return always true
**/
bool getCartesianPosCmd(pr2_controllers::GetCartesianPosCmd::request &req,pr2_controllers::GetCartesianPosCmd::response &resp);
-
+
bool setJointGains(pr2_controllers::SetJointGains::request &req,
pr2_controllers::SetJointGains::response &resp);
Modified: pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/arm_velocity_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/arm_velocity_controller.h 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/arm_velocity_controller.h 2008-09-04 21:54:19 UTC (rev 3954)
@@ -58,11 +58,11 @@
// The maximum number of joints expected in an arm.
static const int MAX_ARM_JOINTS = 7;
-
+
class ArmVelocityController : public Controller
{
public:
-
+
/*!
* \brief Default Constructor of the JointController class.
*
@@ -78,8 +78,8 @@
* \brief Functional way to initialize limits and gains.
*
*/
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
-
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
/*!
* \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
*
@@ -91,7 +91,7 @@
* \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
*/
void getJointVelCmd(pr2_controllers::GetJointVelCmd::response &resp);
-
+
void getCurrentConfiguration(std::vector<double> &);
/*!
* \brief Issues commands to the joint. Should be called at regular intervals
@@ -109,13 +109,13 @@
private:
std::vector<JointVelocityController *> joint_velocity_controllers_;
-
+
// Goal of the joints
std::vector<double> goals_;
// Goal of the joints - used by the realtime code only
- std::vector<double> goals_rt_;
+ std::vector<double> goals_rt_;
- mechanism::Robot* robot_;
+ mechanism::RobotState* robot_;
void updateJointControllers(void);
};
@@ -136,7 +136,7 @@
void update();
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
// Services
bool setJointVelCmd(pr2_controllers::SetJointVelCmd::request &req,
@@ -154,7 +154,7 @@
bool getJointGains(pr2_controllers::GetJointGains::request &req,
pr2_controllers::GetJointGains::response &resp);
-
+
private:
ArmVelocityController *c_;
robot_kinematics::RobotKinematics pr2_kin_;
Modified: pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h 2008-09-04 21:54:19 UTC (rev 3954)
@@ -81,7 +81,7 @@
JointVelocityController controller_; /** controller for the link */
- mechanism::Joint *joint_; /** pointer to joint in Robot structure corresponding to link */
+ mechanism::JointState *joint_; /** pointer to joint in Robot structure corresponding to link */
BaseParam *parent_; /** pointer to parent corresponding to link */
@@ -111,8 +111,8 @@
* \brief Functional way to initialize limits and gains.
*
*/
- void init(std::vector<JointControlParam> jcp, mechanism::Robot *robot);
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ void init(std::vector<JointControlParam> jcp, mechanism::RobotState *robot);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
/*!
@@ -203,7 +203,7 @@
/*!
* \brief Robot representation
*/
- mechanism::Robot* robot_;
+ mechanism::RobotState* robot_;
/*!
@@ -330,7 +330,7 @@
void update();
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
void getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw);
// Services
Modified: pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/laser_scanner_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/laser_scanner_controller.h 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/laser_scanner_controller.h 2008-09-04 21:54:19 UTC (rev 3954)
@@ -71,8 +71,8 @@
* \brief Functional way to initialize limits and gains.
*
*/
- void init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::Robot *robot);
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ void init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::RobotState *robot);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
/*!
* \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
@@ -171,11 +171,11 @@
void setDynamicSawtooth(double time_from_start);
- mechanism::Joint* joint_; /*!< Joint we're controlling>*/
+ mechanism::JointState* joint_; /*!< Joint we're controlling>*/
JointPositionController joint_position_controller_; /*!< Internal PID controller>*/
double last_time_; /*!< Last time stamp of update> */
double command_; /*!< Last commanded position> */
- mechanism::Robot *robot_; /*!< Pointer to robot structure>*/
+ mechanism::RobotState *robot_; /*!< Pointer to robot structure>*/
double* profile_locations_; /**<Contains locations for profile>*/
double* profile_dt_; /**<Contains timesteps for profile locations>*/
int profile_index_; /**<Track location in profile>*/
@@ -211,7 +211,7 @@
void update();
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
// Services
bool setCommand(generic_controllers::SetCommand::request &req,
@@ -222,13 +222,13 @@
bool getActual(generic_controllers::GetActual::request &req,
generic_controllers::GetActual::response &resp);
-
+
bool setProfileCall(generic_controllers::SetProfile::request &req,
generic_controllers::SetProfile::response &resp);
void setCommand(double command);
- void setProfile(LaserScannerController::LaserControllerMode profile, double period, double amplitude, int num_elements=0, double offset=0.0);
+ void setProfile(LaserScannerController::LaserControllerMode profile, double period, double amplitude, int num_elements=0, double offset=0.0);
double getCommand();
Modified: pkg/trunk/controllers/pr2_controllers/src/arm_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_controllers/src/arm_position_controller.cpp 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/pr2_controllers/src/arm_position_controller.cpp 2008-09-04 21:54:19 UTC (rev 3954)
@@ -54,7 +54,7 @@
cout<<"deleted dummy"<<endl;
}
-bool ArmPositionController::initXml(mechanism::Robot * robot, TiXmlElement * config)
+bool ArmPositionController::initXml(mechanism::RobotState * robot, TiXmlElement * config)
{
robot_ = robot;
TiXmlElement *elt = config->FirstChildElement("controller");
@@ -66,12 +66,12 @@
joint_position_controllers_.push_back(jpc);
if(!jpc->initXml(robot_, elt))
return false;
-
+
elt = elt->NextSiblingElement("controller");
}
goals_.resize(joint_position_controllers_.size());
goals_rt_.resize(joint_position_controllers_.size());
-
+
cout<<"CONFIGURE DUMMY "<<joint_position_controllers_.size()<<endl;
return true;
}
@@ -162,10 +162,10 @@
goals_rt_[i] = goals_[i];
arm_controller_lock_.unlock();
}
-
+
for(unsigned int i=0;i<goals_rt_.size();++i)
joint_position_controllers_[i]->setCommand(goals_rt_[i]);
-
+
updateJointControllers();
}
@@ -196,12 +196,12 @@
c_->update();
}
-bool ArmPositionControllerNode::initXml(mechanism::Robot * robot, TiXmlElement * config)
+bool ArmPositionControllerNode::initXml(mechanism::RobotState * robot, TiXmlElement * config)
{
std::cout<<"LOADING ARMCONTROLLERNODE"<<std::endl;
ros::node * const node = ros::node::instance();
string prefix = config->Attribute("name");
-
+
// Parses controller configuration.
std::string kdl_chain_name="";
TiXmlElement *j = config->FirstChildElement("map");
@@ -215,15 +215,15 @@
}
j = j->NextSiblingElement("elem");
}
-
+
// Parses kinematics description
std::string pr2Contents;
node->get_param("robotdesc/pr2", pr2Contents);
pr2_kin_.loadString(pr2Contents.c_str());
arm_chain_ = pr2_kin_.getSerialChain(kdl_chain_name.c_str());
-
+
assert(arm_chain_);
-
+
// Parses subcontroller configuration
if(c_->initXml(robot, config))
{
@@ -238,7 +238,7 @@
return true;
}
- return false;
+ return false;
}
@@ -285,7 +285,7 @@
targetRot = targetRot.RPY(req.roll, req.pitch, req.yaw);
targetFrame.M = targetRot;
std::cout<<"TARGET FRAME"<<targetFrame<<std::endl;
-
+
// Stores the current state in a KDL frame in a realtime safe way
const int size = arm_chain_->num_joints_;
KDL::JntArray cur_cfg = KDL::JntArray(size);
@@ -298,20 +298,20 @@
std::cout<<"CURRENT CFG"<<cur_cfg<<std::endl;
// Setting guess of inverse kinematics
for(int i=0;i<size;++i)
- (*arm_chain_->q_IK_guess)(i) = cur_cfg(i);
+ (*arm_chain_->q_IK_guess)(i) = cur_cfg(i);
// Inverse kinematics:
KDL::JntArray target_cfg = KDL::JntArray(size);
if(!arm_chain_->computeIK(targetFrame))
return false;
target_cfg = *(arm_chain_->q_IK_result);
std::cout<<"TARGET CFG\n"<<target_cfg<<std::endl;
-
-
+
+
//TODO: check IK result
if(!arm_chain_->computeFK(target_cfg, targetFrame))
return false;
std::cout<<"Positions seems correct!"<<std::endl;
-
+
// Sends commands to the controllers
pr2_controllers::SetJointPosCmd::request commands;
commands.set_positions_size(size);
Modified: pkg/trunk/controllers/pr2_controllers/src/arm_velocity_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_controllers/src/arm_velocity_controller.cpp 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/pr2_controllers/src/arm_velocity_controller.cpp 2008-09-04 21:54:19 UTC (rev 3954)
@@ -54,7 +54,7 @@
cout<<"deleted dummy"<<endl;
}
-bool ArmVelocityController::initXml(mechanism::Robot * robot, TiXmlElement * config)
+bool ArmVelocityController::initXml(mechanism::RobotState * robot, TiXmlElement * config)
{
robot_ = robot;
TiXmlElement *elt = config->FirstChildElement("controller");
@@ -66,12 +66,12 @@
joint_velocity_controllers_.push_back(jpc);
if(!jpc->initXml(robot_, elt))
return false;
-
+
elt = elt->NextSiblingElement("controller");
}
goals_.resize(joint_velocity_controllers_.size());
goals_rt_.resize(joint_velocity_controllers_.size());
-
+
cout<<"CONFIGURE DUMMY "<<joint_velocity_controllers_.size()<<endl;
return true;
}
@@ -152,10 +152,10 @@
goals_rt_[i] = goals_[i];
arm_controller_lock_.unlock();
}
-
+
for(unsigned int i=0;i<goals_rt_.size();++i)
joint_velocity_controllers_[i]->setCommand(goals_rt_[i]);
-
+
updateJointControllers();
}
@@ -186,12 +186,12 @@
c_->update();
}
-bool ArmVelocityControllerNode::initXml(mechanism::Robot * robot, TiXmlElement * config)
+bool ArmVelocityControllerNode::initXml(mechanism::RobotState * robot, TiXmlElement * config)
{
std::cout<<"LOADING ARMCONTROLLERNODE"<<std::endl;
ros::node * const node = ros::node::instance();
string prefix = config->Attribute("name");
-
+
// Parses controller configuration.
std::string kdl_chain_name="";
TiXmlElement *j = config->FirstChildElement("map");
@@ -205,15 +205,15 @@
}
j = j->NextSiblingElement("elem");
}
-
+
// Parses kinematics description
std::string pr2Contents;
node->get_param("robotdesc/pr2", pr2Contents);
pr2_kin_.loadString(pr2Contents.c_str());
arm_chain_ = pr2_kin_.getSerialChain(kdl_chain_name.c_str());
-
+
assert(arm_chain_);
-
+
// Parses subcontroller configuration
if(c_->initXml(robot, config))
{
@@ -227,7 +227,7 @@
node->advertise_service(prefix + "/get_cartesian_vel", &ArmVelocityControllerNode::getCartesianVelCmd, this);
return true;
}
- return false;
+ return false;
}
@@ -280,7 +280,7 @@
arm_chain_->computeDKInv(cur_joint_pos,end_effector_twist,target_joint_vel);
std::cout << "TARGET VEL\n" << target_joint_vel << std::endl;
-
+
/* // Sends commands to the controllers
pr2_controllers::SetJointVelCmd::request commands;
commands.set_velocity_size(size);
Modified: pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-09-04 21:54:19 UTC (rev 3954)
@@ -94,7 +94,7 @@
};
-void BaseController::init(std::vector<JointControlParam> jcp, mechanism::Robot *robot)
+void BaseController::init(std::vector<JointControlParam> jcp, mechanism::RobotState *robot)
{
std::vector<JointControlParam>::iterator jcp_iter;
robot_desc::URDF::Link *link;
@@ -117,9 +117,11 @@
base_object.pos_.z = link->xyz[2];
base_object.name_ = joint_name;
base_object.parent_ = NULL;
- base_object.joint_ = robot->getJoint(joint_name);
- base_object.controller_.init(jcp_iter->p_gain,jcp_iter->i_gain,jcp_iter->d_gain,jcp_iter->windup,0.0,jcp_iter->joint_name,robot);
+ base_object.joint_ = robot->getJointState(joint_name);
+ abort();
+ // TODO base_object.controller_.init(jcp_iter->p_gain,jcp_iter->i_gain,jcp_iter->d_gain,jcp_iter->windup,0.0,jcp_iter->joint_name,robot);
+
if(joint_name.find("caster") != string::npos)
{
base_object.local_id_ = num_casters_;
@@ -176,7 +178,7 @@
last_time_ = robot_->hw_->current_time_;
}
-bool BaseController::initXml(mechanism::Robot *robot, TiXmlElement *config)
+bool BaseController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
std::cout << " base controller name: " << config->Attribute("name") << std::endl;
TiXmlElement *elt = config->FirstChildElement("controller");
@@ -257,7 +259,7 @@
res1 += base_wheels_[i].parent_->pos_;
// cout << "added position" << res1;
base_wheels_position_[i] = res1;
-// cout << "base_wheels_position_(" << i << ")" << base_wheels_position_[i];
+// cout << "base_wheels_position_(" << i << ")" << base_wheels_position_[i];
}
// exit(-1);
}
@@ -414,14 +416,14 @@
return true;
}
-bool BaseControllerNode::initXml(mechanism::Robot *robot, TiXmlElement *config)
+bool BaseControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
ros::node *node = ros::node::instance();
string prefix = config->Attribute("name");
if(!c_->initXml(robot, config))
return false;
-
+
node->advertise_service(prefix + "/set_command", &BaseControllerNode::setCommand, this);
node->advertise_service(prefix + "/get_actual", &BaseControllerNode::getCommand, this); //FIXME: this is actually get command, just returning command for testing.
@@ -485,7 +487,7 @@
odom_msg_.header.frame_id = "FRAMEID_ODOM";
odom_msg_.header.stamp.sec = (unsigned long)floor(robot_->hw_->current_time_);
odom_msg_.header.stamp.nsec = (unsigned long)floor( 1e9 * ( robot_->hw_->current_time_ - odom_msg_.header.stamp.sec) );
-
+
odom_msg_.pos.x = base_odom_position_.x;
odom_msg_.pos.y = base_odom_position_.y;
odom_msg_.pos.th = base_odom_position_.z;
@@ -511,12 +513,12 @@
A.element(i*2+1,0) = sin(steer_angle)*wheel_radius_*(wheel_speed_actual_[i]);
// if(isnan(steer_angle))
// {
-// cout << "steer angle - nan for wheel " << i << endl;
+// cout << "steer angle - nan for wheel " << i << endl;
// exit(-1);
// }
// if(isnan(wheel_speed_actual_[i]))
// {
-// cout << "wheel speed actual - nan for wheel " << i << endl;
+// cout << "wheel speed actual - nan for wheel " << i << endl;
// exit(-1);
// }
}
@@ -530,7 +532,7 @@
C.element(i*2+1, 2) = base_wheels_position_[i].x;
// if(isnan(base_wheels_position_[i].y) || isnan(base_wheels_position_[i].x))
// {
-// cout << "base odom position - nan for wheel " << i << endl;
+// cout << "base odom position - nan for wheel " << i << endl;
// exit(-1);
// }
}
@@ -538,7 +540,7 @@
// if(isnan(base_odom_velocity_.x) || isnan(base_odom_velocity_.y) || isnan(base_odom_velocity_.z))
// {
-// cout << "base odom velocity - nan" << endl;
+// cout << "base odom velocity - nan" << endl;
// exit(-1);
// }
@@ -564,7 +566,7 @@
std::ostream & controller::operator<<(std::ostream& mystream, const controller::BaseParam &bp)
{
- mystream << "BaseParam" << bp.name_ << endl << "position " << bp.pos_ << "id " << bp.local_id_ << endl << "joint " << bp.joint_->name_ << endl << endl;
+ mystream << "BaseParam" << bp.name_ << endl << "position " << bp.pos_ << "id " << bp.local_id_ << endl << "joint " << bp.joint_->joint_->name_ << endl << endl;
return mystream;
};
Modified: pkg/trunk/controllers/pr2_controllers/src/laser_scanner_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_controllers/src/laser_scanner_controller.cpp 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/pr2_controllers/src/laser_scanner_controller.cpp 2008-09-04 21:54:19 UTC (rev 3954)
@@ -65,17 +65,18 @@
}
-void LaserScannerController::init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::Robot *robot)
+void LaserScannerController::init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::RobotState *robot)
{
robot_ = robot;
- joint_ = robot->getJoint(name);
+ joint_ = robot->getJointState(name);
- joint_position_controller_.init( p_gain, i_gain, d_gain, windup, time, name, robot);
+ // TODO
+ abort(); //joint_position_controller_.init( p_gain, i_gain, d_gain, windup, time, name, robot);
command_= 0;
last_time_= time;
}
-bool LaserScannerController::initXml(mechanism::Robot *robot, TiXmlElement *config)
+bool LaserScannerController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
assert(robot);
robot_ = robot;
@@ -90,7 +91,7 @@
}
const char *joint_name = j->Attribute("name");
- joint_ = joint_name ? robot->getJoint(joint_name) : NULL;
+ joint_ = joint_name ? robot->getJointState(joint_name) : NULL;
if (!joint_)
{
fprintf(stderr, "LaserScannerController could not find joint named \"%s\"\n", joint_name);
@@ -353,7 +354,7 @@
void LaserScannerController::setJointEffort(double effort)
{
- joint_->commanded_effort_ = min(max(effort, -joint_->effort_limit_), joint_->effort_limit_);
+ joint_->commanded_effort_ = effort;
}
//Get sinewave based on current time
@@ -361,7 +362,7 @@
{
double command = sin(2*M_PI*time_from_start/period_)*amplitude_+offset_;
joint_position_controller_.setCommand(command);
-
+
}
//Set mode to use sawtooth profile
@@ -450,11 +451,11 @@
generic_controllers::SetProfile::response &resp)
{
setProfile(LaserScannerController::LaserControllerMode(req.profile),req.period,req.amplitude,req.offset);
- resp.time = c_->getTime();
+ resp.time = c_->getTime();
return true;
}
-bool LaserScannerControllerNode::initXml(mechanism::Robot *robot, TiXmlElement *config)
+bool LaserScannerControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
ros::node *node = ros::node::instance();
string prefix = config->Attribute("name");
Modified: pkg/trunk/controllers/pr2_controllers/test/test_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_controllers/test/test_base_controller.cpp 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/pr2_controllers/test/test_base_controller.cpp 2008-09-04 21:54:19 UTC (rev 3954)
@@ -6,8 +6,9 @@
int main( int argc, char** argv )
{
- mechanism::Robot *robot = new mechanism::Robot("r2d2");
+ mechanism::Robot *robot_model = new mechanism::Robot;
controller::BaseController bc;
+ HardwareInterface hw(0);
if(argc != 3)
{
@@ -23,18 +24,21 @@
TiXmlDocument xml(xml_robot_file); // Load robot description
xml.LoadFile();
TiXmlElement *root = xml.FirstChildElement("robot");
- robot->initXml(root);
-
+ robot_model->initXml(root);
+
char *xml_control_file = argv[2];
TiXmlDocument xml_control(xml_control_file); // Load robot description
xml_control.LoadFile();
TiXmlElement *root_control = xml_control.FirstChildElement("robot");
TiXmlElement *root_controller = root_control->FirstChildElement("controller");
- bc.initXml(robot,root_controller);
+ mechanism::RobotState *robot_state = new mechanism::RobotState(robot_model, &hw);
+
+ bc.initXml(robot_state,root_controller);
ros::fini();
- delete robot;
+ delete robot_model;
+ delete robot_state;
//void BaseController::initXml(mechanism::Robot *robot, TiXmlElement *config)
}
Modified: pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test1.h
===================================================================
--- pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test1.h 2008-09-04 21:04:47 UTC (rev 3953)
+++ pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test1.h 2008-09-04 21:54:19 UTC (rev 3954)
@@ -73,9 +73,8 @@
* \param *robot The robot that is being controlled.
*/
void init(double duration, std::string fixture_name, double time, std::string name,mechanism::Robot *robot);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- bool initXml(mechanism::Robot *robot, TiXmlElement *config);
-
/*!
* \brief Get latest time..
*/
@@ -93,6 +92,7 @@
virtual void update();
private:
+<<<<<<< HEAD:controllers/testing_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test1.h
mechanism::Joint* joint_; /**< Joint we're controlling. */
mechanism::Joint* fixture_joint_; /**< Joint we're qualifying against. */
mechanism::Robot *robot_; /**< Pointer to robot structure. */
@@ -103,6 +103,14 @@
double test_joint_end_pos_; /**< End pos of the test joint. */
double fixture_joint_end_pos_; /**< End pos of the fixture joint. */
bool complete;
+=======
+ mechanism::Joint::State *joint_state_;
+ mechanism::RobotState *robot_; ...
[truncated message content] |
