Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22790] pkg/trunk
|
From: <mee...@us...> - 2009-08-24 23:54:44
|
Revision: 22790
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22790&view=rev
Author: meeussen
Date: 2009-08-24 23:54:35 +0000 (Mon, 24 Aug 2009)
Log Message:
-----------
Port mechanism model, kdl parser, robot_state_publisher to new urdf format. Update api of mechanism model. Door test works
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pr2_gripper_controller.h
pkg/trunk/sandbox/experimental_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_pose_twist_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_spline_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_tff_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_twist_controller_ik.cpp
pkg/trunk/sandbox/experimental_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_autotuner.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_calibration_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_chain_constraint_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_chain_sine_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_inverse_dynamics_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_limit_calibration_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_pd_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_position_smoothing_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/pid_position_velocity_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/pr2_gripper_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/probe.cpp
pkg/trunk/sandbox/experimental_controllers/src/trajectory_controller.cpp
pkg/trunk/stacks/mechanism/kdl_parser/CMakeLists.txt
pkg/trunk/stacks/mechanism/kdl_parser/manifest.xml
pkg/trunk/stacks/mechanism/kdl_parser/test/test_kdl_parser.cpp
pkg/trunk/stacks/mechanism/mechanism_control/manifest.xml
pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/stacks/mechanism/mechanism_control/src/recorder.cpp
pkg/trunk/stacks/mechanism/mechanism_model/CMakeLists.txt
pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h
pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/joint.h
pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/robot.h
pkg/trunk/stacks/mechanism/mechanism_model/manifest.xml
pkg/trunk/stacks/mechanism/mechanism_model/src/chain.cpp
pkg/trunk/stacks/mechanism/mechanism_model/src/gripper_transmission.cpp
pkg/trunk/stacks/mechanism/mechanism_model/src/joint.cpp
pkg/trunk/stacks/mechanism/mechanism_model/src/pr2_gripper_transmission.cpp
pkg/trunk/stacks/mechanism/mechanism_model/src/robot.cpp
pkg/trunk/stacks/mechanism/mechanism_model/src/simple_transmission.cpp
pkg/trunk/stacks/mechanism/mechanism_model/test/test_chain.cpp
pkg/trunk/stacks/mechanism/mechanism_model/test/test_helpers.h
pkg/trunk/stacks/mechanism/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h
pkg/trunk/stacks/mechanism/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h
pkg/trunk/stacks/mechanism/robot_state_publisher/manifest.xml
pkg/trunk/stacks/mechanism/robot_state_publisher/src/joint_state_listener.cpp
pkg/trunk/stacks/mechanism/robot_state_publisher/src/state_publisher.cpp
pkg/trunk/stacks/mechanism/robot_state_publisher/test/test_publisher.cpp
pkg/trunk/stacks/pr2/pr2_alpha/pre_ethercat_reset.launch
pkg/trunk/stacks/pr2/pr2_alpha/prf_ethercat_reset.launch
pkg/trunk/stacks/pr2/pr2_alpha/prg_ethercat_reset.launch
pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp
pkg/trunk/stacks/pr2_common/pr2_defs/defs/arm/arm_defs.urdf.xacro
pkg/trunk/stacks/pr2_simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
pkg/trunk/stacks/robot_model/urdf/include/urdf/joint.h
pkg/trunk/stacks/robot_model/urdf/include/urdf/model.h
pkg/trunk/stacks/robot_model/urdf/src/joint.cpp
pkg/trunk/stacks/simulator_gazebo/gazebo/launch/empty_world.launch
Added Paths:
-----------
pkg/trunk/stacks/mechanism/kdl_parser/include/kdl_parser/dom_parser.hpp
pkg/trunk/stacks/mechanism/kdl_parser/include/kdl_parser/xml_parser.hpp
pkg/trunk/stacks/mechanism/kdl_parser/pr2.urdf
pkg/trunk/stacks/mechanism/kdl_parser/src/dom_parser.cpp
pkg/trunk/stacks/mechanism/kdl_parser/src/xml_parser.cpp
pkg/trunk/stacks/mechanism/kdl_parser/test/example_dom.cpp
pkg/trunk/stacks/mechanism/kdl_parser/test/example_xml.cpp
Removed Paths:
-------------
pkg/trunk/stacks/mechanism/kdl_parser/include/kdl_parser/tree_parser.hpp
pkg/trunk/stacks/mechanism/kdl_parser/src/tree_parser.cpp
pkg/trunk/stacks/mechanism/kdl_parser/test/example.cpp
pkg/trunk/stacks/mechanism/mechanism_model/pr2_desc.xml
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-08-24 23:54:35 UTC (rev 22790)
@@ -41,8 +41,8 @@
//#include <robot_mechanism_controllers/joint_position_smoothing_controller.h>
#include <realtime_tools/realtime_publisher.h>
+#include <tf/tf.h>
-
#include "filters/transfer_function.h"
// Messages
@@ -102,7 +102,7 @@
//boost::mutex track_link_lock_ ;
//bool track_link_enabled_ ;
//mechanism::LinkState* target_link_ ;
- mechanism::LinkState* mount_link_ ;
+ //mechanism::LinkState* mount_link_ ;
tf::Vector3 track_point_ ;
//JointPositionSmoothController joint_position_controller_ ; // The PID position controller that is doing all the under-the-hood controls stuff
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h 2009-08-24 23:54:35 UTC (rev 22790)
@@ -37,6 +37,7 @@
#include <nav_msgs/Odometry.h>
#include <realtime_tools/realtime_publisher.h>
#include <tf/tfMessage.h>
+#include <tf/tf.h>
#include <pr2_mechanism_controllers/base_kinematics.h>
#include <angles/angles.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-08-24 23:54:35 UTC (rev 22790)
@@ -28,6 +28,7 @@
<depend package="pluginlib" />
<depend package="rosconsole" />
<depend package="eigen" />
+ <depend package="tf" />
<depend package="angles" />
<depend package="control_toolbox" />
<depend package="filters" />
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -49,21 +49,23 @@
wheel_speed_cmd_ = 0;
wheel_speed_actual_ = 0;
- mechanism::Link *link = robot_state->model_->getLink(link_name);
- if(!link)
- {
+ KDL::SegmentMap::const_iterator link;
+ try{
+ link = robot_state->model_->robot_model_.getSegment(link_name);
+ }
+ catch (KDL::Tree::segment_name_exception& ex){
ROS_ERROR("Could not find link with name %s",link_name.c_str());
return false;
}
ROS_DEBUG("wheel name: %s",link_name.c_str());
link_name_ = link_name;
- joint_name_ = link->joint_name_;
+ joint_name_ = link->second.segment.getJoint().getName();
joint_ = robot_state->getJointState(joint_name_);
- tf::Transform offset = link->getOffset();
- offset_.x = offset.getOrigin().x();
- offset_.y = offset.getOrigin().y();
- offset_.z = offset.getOrigin().z();
+ KDL::Vector offset = link->second.segment.pose(0.0).p;
+ offset_.x = offset.x();
+ offset_.y = offset.y();
+ offset_.z = offset.z();
node.param<double> ("wheel_radius_scaler", wheel_radius_scaler_, 1.0);
ROS_DEBUG("Loading wheel: %s",link_name_.c_str());
ROS_DEBUG("offset_.x: %f, offset_.y: %f, offset_.z: %f", offset_.x, offset_.y, offset_.z);
@@ -82,26 +84,27 @@
steer_angle_actual_ = 0;
num_children_ = 0;
- ROS_DEBUG("caster name: %s",link_name.c_str());
- link_name_ = link_name;
- mechanism::Link *link = robot_state->model_->getLink(link_name);
- if(!link)
- {
+ KDL::SegmentMap::const_iterator link;
+ try{
+ link = robot_state->model_->robot_model_.getSegment(link_name);
+ }
+ catch (KDL::Tree::segment_name_exception& ex){
ROS_ERROR("Could not find link with name %s",link_name.c_str());
return false;
}
- joint_name_ = link->joint_name_;
+ ROS_DEBUG("caster name: %s",link_name.c_str());
+ link_name_ = link_name;
+
+ joint_name_ = link->second.segment.getJoint().getName();
joint_ = robot_state->getJointState(joint_name_);
- tf::Transform offset = link->getOffset();
- offset_.x = offset.getOrigin().x();
- offset_.y = offset.getOrigin().y();
- offset_.z = offset.getOrigin().z();
+ KDL::Vector offset = link->second.segment.pose(0.0).p;
+ offset_.x = offset.x();
+ offset_.y = offset.y();
+ offset_.z = offset.z();
- KDL::SegmentMap::const_iterator root = robot_state->model_->tree_.getSegment(link_name);
-
- for(unsigned int i=0; i < root->second.children.size(); i++)
+ for(unsigned int i=0; i < link->second.children.size(); i++)
{
- KDL::SegmentMap::const_iterator child = root->second.children[i];
+ KDL::SegmentMap::const_iterator child = link->second.children[i];
Wheel tmp;
parent_->wheel_.push_back(tmp);
if(!parent_->wheel_[parent_->num_wheels_].init(robot_state, node.getNamespace(), child->second.segment.getName()))
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -248,11 +248,11 @@
if (pub_calibrated_)
{
- if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ if (last_publish_time_ + 0.5 < robot_->getTime())
{
if (pub_calibrated_->trylock())
{
- last_publish_time_ = robot_->hw_->current_time_;
+ last_publish_time_ = robot_->getTime();
pub_calibrated_->unlockAndPublish();
}
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -190,11 +190,11 @@
case CALIBRATED:
if (pub_calibrated_)
{
- if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ if (last_publish_time_ + 0.5 < robot_->getTime())
{
if (pub_calibrated_->trylock())
{
- last_publish_time_ = robot_->hw_->current_time_;
+ last_publish_time_ = robot_->getTime();
pub_calibrated_->unlockAndPublish();
}
}
@@ -224,12 +224,12 @@
if (c_.calibrated())
{
- if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ if (last_publish_time_ + 0.5 < robot_->getTime())
{
assert(pub_calibrated_);
if (pub_calibrated_->trylock())
{
- last_publish_time_ = robot_->hw_->current_time_;
+ last_publish_time_ = robot_->getTime();
pub_calibrated_->unlockAndPublish();
}
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -104,7 +104,7 @@
ROS_ERROR("%s:: Error initializing pid element", name_.c_str()) ;
return false ;
}
- last_time_ = robot->hw_->current_time_ ;
+ last_time_ = robot->getTime() ;
last_error_ = 0.0 ;
// ***** Derivative Error Filter Element *****
@@ -216,7 +216,7 @@
// ***** Run the position control loop *****
double cmd = traj_command_ + tracking_offset_ ;
- double time = robot_->hw_->current_time_ ;
+ double time = robot_->getTime() ;
double error(0.0) ;
angles::shortest_angular_distance_with_limits(cmd, joint_state_->position_,
joint_state_->joint_->joint_limit_min_,
@@ -237,7 +237,7 @@
double LaserScannerTrajController::getCurProfileTime()
{
- double time = robot_->hw_->current_time_ ;
+ double time = robot_->getTime() ;
double time_from_start = time - traj_start_time_ ;
double mod_time = time_from_start - floor(time_from_start/traj_.getTotalTime())*traj_.getTotalTime() ;
return mod_time ;
@@ -273,7 +273,7 @@
traj_.setTrajectory(traj_points) ;
- traj_start_time_ = robot_->hw_->current_time_ ;
+ traj_start_time_ = robot_->getTime() ;
traj_duration_ = traj_.getTotalTime() ;
@@ -291,8 +291,8 @@
double low_pt = -cmd.amplitude + cmd.offset ;
- double soft_limit_low = joint_state_->joint_->joint_limit_min_ + joint_state_->joint_->safety_length_min_ ;
- double soft_limit_high = joint_state_->joint_->joint_limit_max_ - joint_state_->joint_->safety_length_max_ ;
+ double soft_limit_low = joint_state_->joint_->safety_limit_min_;
+ double soft_limit_high = joint_state_->joint_->safety_limit_max_;
if (low_pt < soft_limit_low)
{
@@ -461,7 +461,7 @@
{
// Should we be populating header.stamp here? Or, we can simply let ros take care of the timestamp
ros::Time cur_time ;
- cur_time.fromSec(robot_->hw_->current_time_) ;
+ cur_time.fromSec(robot_->getTime()) ;
m_scanner_signal_.header.stamp = cur_time ;
m_scanner_signal_.signal = cur_profile_segment ;
need_to_send_msg_ = true ;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -171,7 +171,7 @@
cmd_vel_t_.linear.y = 0.0;
}
cmd_vel_t_.angular.z = filters::clamp(cmd_vel.angular.z, -max_vel_.angular.z, max_vel_.angular.z);
- cmd_received_timestamp_ = base_kin_.robot_state_->hw_->current_time_;
+ cmd_received_timestamp_ = base_kin_.robot_state_->getTime();
ROS_DEBUG("BaseController:: command received: %f %f %f",cmd_vel.linear.x,cmd_vel.linear.y,cmd_vel.angular.z);
ROS_DEBUG("BaseController:: command current: %f %f %f", cmd_vel_.linear.x,cmd_vel_.linear.y,cmd_vel_.angular.z);
@@ -250,8 +250,8 @@
}
}
- last_time_ = base_kin_.robot_state_->hw_->current_time_;
- cmd_received_timestamp_ = base_kin_.robot_state_->hw_->current_time_;
+ last_time_ = base_kin_.robot_state_->getTime();
+ cmd_received_timestamp_ = base_kin_.robot_state_->getTime();
for(int i = 0; i < base_kin_.num_casters_; i++)
{
caster_controller_[i]->starting();
@@ -266,7 +266,7 @@
void Pr2BaseController::update()
{
- double current_time = base_kin_.robot_state_->hw_->current_time_;
+ double current_time = base_kin_.robot_state_->getTime();
double dT = std::min<double>(current_time - last_time_, base_kin_.MAX_DT_);
if(new_cmd_available_)
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -105,15 +105,15 @@
bool Pr2Odometry::starting()
{
- current_time_ = base_kin_.robot_state_->hw_->current_time_;
- last_time_ = base_kin_.robot_state_->hw_->current_time_;
- last_publish_time_ = base_kin_.robot_state_->hw_->current_time_;
+ current_time_ = base_kin_.robot_state_->getTime();
+ last_time_ = base_kin_.robot_state_->getTime();
+ last_publish_time_ = base_kin_.robot_state_->getTime();
return true;
}
void Pr2Odometry::update()
{
- current_time_ = base_kin_.robot_state_->hw_->current_time_;
+ current_time_ = base_kin_.robot_state_->getTime();
updateOdometry();
publish();
last_time_ = current_time_;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -360,12 +360,12 @@
if (pub_calibrated_)
{
- if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ if (last_publish_time_ + 0.5 < robot_->getTime())
{
assert(pub_calibrated_);
if (pub_calibrated_->trylock())
{
- last_publish_time_ = robot_->hw_->current_time_;
+ last_publish_time_ = robot_->getTime();
pub_calibrated_->unlockAndPublish();
}
}
@@ -403,12 +403,12 @@
if (c_.calibrated())
{
- if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ if (last_publish_time_ + 0.5 < robot_->getTime())
{
assert(pub_calibrated_);
if (pub_calibrated_->trylock())
{
- last_publish_time_ = robot_->hw_->current_time_;
+ last_publish_time_ = robot_->getTime();
pub_calibrated_->unlockAndPublish();
}
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h 2009-08-24 23:54:35 UTC (rev 22790)
@@ -63,8 +63,8 @@
#include <controller_interface/controller.h>
#include <control_toolbox/pid.h>
#include "control_toolbox/pid_gains_setter.h"
+#include <boost/scoped_ptr.hpp>
#include <realtime_tools/realtime_publisher.h>
-
#include <std_msgs/Float64.h>
#include <robot_mechanism_controllers/JointControllerState.h>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h 2009-08-24 23:54:35 UTC (rev 22790)
@@ -61,6 +61,7 @@
#include <ros/node.h>
#include <ros/node_handle.h>
+#include <boost/scoped_ptr.hpp>
#include "controller_interface/controller.h"
#include "control_toolbox/pid.h"
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -127,7 +127,7 @@
// initialize desired pose/twist
twist_ff_ = Twist::Zero();
pose_desi_ = getPose();
- last_time_ = robot_state_->hw_->current_time_;
+ last_time_ = robot_state_->getTime();
loop_count_ = 0;
return true;
@@ -138,7 +138,7 @@
void CartesianPoseController::update()
{
// get time
- double time = robot_state_->hw_->current_time_;
+ double time = robot_state_->getTime();
double dt = time - last_time_;
last_time_ = time;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -142,7 +142,7 @@
fb_pid_controller_[i].reset();
// time
- last_time_ = robot_state_->hw_->current_time_;
+ last_time_ = robot_state_->getTime();
// set disired twist to 0
twist_desi_ = Twist::Zero();
@@ -159,7 +159,7 @@
return;
// get time
- double time = robot_state_->hw_->current_time_;
+ double time = robot_state_->getTime();
double dt = time - last_time_;
last_time_ = time;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -57,7 +57,7 @@
{
assert(robot);
robot_ = robot;
- last_time_ = robot->hw_->current_time_;
+ last_time_ = robot->getTime();
joint_state_ = robot_->getJointState(joint_name);
if (!joint_state_)
@@ -158,7 +158,7 @@
assert(robot_ != NULL);
double error(0);
- double time = robot_->hw_->current_time_;
+ double time = robot_->getTime();
assert(joint_state_->joint_);
dt_= time - last_time_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -244,12 +244,12 @@
case CALIBRATED:
if (pub_calibrated_)
{
- if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ if (last_publish_time_ + 0.5 < robot_->getTime())
{
assert(pub_calibrated_);
if (pub_calibrated_->trylock())
{
- last_publish_time_ = robot_->hw_->current_time_;
+ last_publish_time_ = robot_->getTime();
pub_calibrated_->unlockAndPublish();
}
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -55,7 +55,7 @@
{
assert(robot);
robot_ = robot;
- last_time_ = robot->hw_->current_time_;
+ last_time_ = robot->getTime();
joint_state_ = robot_->getJointState(joint_name);
if (!joint_state_)
@@ -159,7 +159,7 @@
void JointVelocityController::update()
{
assert(robot_ != NULL);
- double time = robot_->hw_->current_time_;
+ double time = robot_->getTime();
double error = joint_state_->velocity_ - command_;
dt_ = time - last_time_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -76,7 +76,7 @@
double TriggerController::getTick()
{
- return getTick(robot_->hw_->current_time_, config_);
+ return getTick(robot_->getTime(), config_);
}
void TriggerController::update()
@@ -101,7 +101,7 @@
}
//if (actuator_command_->digital_out_ && !(active ^ config_.active_low))
- // ROS_DEBUG("digital out falling at time %f", robot_->hw_->current_time_);
+ // ROS_DEBUG("digital out falling at time %f", robot_->getTime());
actuator_command_->digital_out_ = active ^ config_.active_low;
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h 2009-08-24 23:54:35 UTC (rev 22790)
@@ -126,6 +126,7 @@
std::vector<int> joint_type_;
mechanism::Robot* robot_;
+ mechanism::RobotState* robot_state_;
void updateJointControllers(void);
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h 2009-08-24 23:54:35 UTC (rev 22790)
@@ -62,6 +62,7 @@
#include <ros/node.h>
#include <ros/node_handle.h>
+#include <boost/scoped_ptr.hpp>
#include "controller_interface/controller.h"
#include "control_toolbox/pid.h"
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pr2_gripper_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pr2_gripper_controller.h 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pr2_gripper_controller.h 2009-08-24 23:54:35 UTC (rev 22790)
@@ -49,6 +49,7 @@
#include "ros/callback_queue.h"
#include <boost/shared_ptr.hpp>
+#include <boost/scoped_ptr.hpp>
namespace controller
{
Modified: pkg/trunk/sandbox/experimental_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/arm_trajectory_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/arm_trajectory_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -64,6 +64,7 @@
ROS_INFO("Initializing trajectory controller");
robot_ = robot->model_;
+ robot_state_ = robot;
mechanism::Joint *joint;
// std::vector<double> joint_velocity_limits;
std::vector<trajectory::Trajectory::TPoint> trajectory_points_vector;
@@ -139,7 +140,7 @@
if(!joint_trajectory_->setTrajectory(trajectory_points_vector))
ROS_WARN("Trajectory not set correctly");
- trajectory_start_time_ = robot_->hw_->current_time_;
+ trajectory_start_time_ = robot_state_->getTime();
trajectory_end_time_ = trajectory_start_time_;
ROS_INFO("ArmTrajectoryController:: Done loading controller");
@@ -199,11 +200,11 @@
double sample_time(0.0);
- current_time_ = robot_->hw_->current_time_;
+ current_time_ = robot_state_->getTime();
if(refresh_rt_vals_)
{
- trajectory_start_time_ = robot_->hw_->current_time_;
+ trajectory_start_time_ = robot_state_->getTime();
trajectory_wait_time_ = 0.0;
trajectory_end_time_ = joint_trajectory_->getTotalTime()+trajectory_start_time_;
refresh_rt_vals_ = false;
@@ -211,7 +212,7 @@
}
if(arm_controller_lock_.try_lock())
{
- sample_time = robot_->hw_->current_time_ - trajectory_start_time_;
+ sample_time = robot_state_->getTime() - trajectory_start_time_;
joint_trajectory_->sample(trajectory_point_,sample_time);
// ROS_INFO("sample_time: %f", sample_time);
@@ -224,9 +225,9 @@
// cout << endl;
arm_controller_lock_.unlock();
- if(robot_->hw_->current_time_ >= trajectory_end_time_ && trajectory_done_ == false)
+ if(robot_state_->getTime() >= trajectory_end_time_ && trajectory_done_ == false)
{
- trajectory_wait_time_ = robot_->hw_->current_time_ - trajectory_end_time_;
+ trajectory_wait_time_ = robot_state_->getTime() - trajectory_end_time_;
if(reachedGoalPosition(joint_cmd_rt_))
{
trajectory_wait_time_ = 0.0;
@@ -440,7 +441,7 @@
delete diagnostics_publisher_ ;
diagnostics_publisher_ = new realtime_tools::RealtimePublisher <diagnostic_msgs::DiagnosticArray> ("/diagnostics", 2) ;
- last_diagnostics_publish_time_ = c_->robot_->hw_->current_time_;
+ last_diagnostics_publish_time_ = c_->robot_state_->getTime();
node_->param<double>(service_prefix_ + "/diagnostics_publish_delta_time",diagnostics_publish_delta_time_,0.05);
ROS_DEBUG("Initialized publisher");
Modified: pkg/trunk/sandbox/experimental_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/cartesian_hybrid_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/cartesian_hybrid_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -250,7 +250,7 @@
{
if (!chain_.allCalibrated(robot_->joint_states_))
return;
- double time = robot_->hw_->current_time_;
+ double time = robot_->getTime();
double dt = time - last_time_;
last_time_ = time;
Modified: pkg/trunk/sandbox/experimental_controllers/src/cartesian_pose_twist_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/cartesian_pose_twist_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/cartesian_pose_twist_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -159,7 +159,7 @@
twist_ff_ = Twist::Zero();
pose_desi_ = getPose();
twist_desi_ = Twist::Zero();
- last_time_ = robot_state_->hw_->current_time_;
+ last_time_ = robot_state_->getTime();
loop_count_ = 0;
return true;
@@ -168,7 +168,7 @@
void CartesianPoseTwistController::update()
{
// get time
- double time = robot_state_->hw_->current_time_;
+ double time = robot_state_->getTime();
double dt = time - last_time_;
last_time_ = time;
Modified: pkg/trunk/sandbox/experimental_controllers/src/cartesian_spline_trajectory_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/cartesian_spline_trajectory_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/cartesian_spline_trajectory_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -136,7 +136,7 @@
new_cmd_available_ = false;
trajectory_status_ = manipulation_srvs::QuerySplineTraj::Response::State_Done;
spline_done_ = false;
- last_time_ = robot_state_->hw_->current_time_;
+ last_time_ = robot_state_->getTime();
goal_ = getCommand(spline_rt_.segments.back(), spline_rt_.segments.back().duration.toSec());
spline_time_ = spline_rt_.segments.back().duration.toSec();
setGoalPose(goal_);
@@ -147,7 +147,7 @@
void CartesianSplineTrajectoryController::update()
{
- spline_time_ += (robot_state_->hw_->current_time_ - last_time_);
+ spline_time_ += (robot_state_->getTime() - last_time_);
if(new_cmd_available_)
{
@@ -190,7 +190,7 @@
setCommand(joint_cmd);
- last_time_ = robot_state_->hw_->current_time_;
+ last_time_ = robot_state_->getTime();
}
manipulation_msgs::Waypoint CartesianSplineTrajectoryController::getCommand(const manipulation_msgs::SplineTrajSegment &spline, const double t)
Modified: pkg/trunk/sandbox/experimental_controllers/src/cartesian_tff_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/cartesian_tff_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/cartesian_tff_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -144,7 +144,7 @@
bool CartesianTFFController::starting()
{
// time
- last_time_ = robot_state_->hw_->current_time_;
+ last_time_ = robot_state_->getTime();
// set initial modes and values
for (unsigned int i=0; i<6; i++){
@@ -173,7 +173,7 @@
void CartesianTFFController::update()
{
// get time
- double time = robot_state_->hw_->current_time_;
+ double time = robot_state_->getTime();
double dt = time - last_time_;
last_time_ = time;
Modified: pkg/trunk/sandbox/experimental_controllers/src/cartesian_trajectory_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/cartesian_trajectory_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/cartesian_trajectory_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -185,7 +185,7 @@
bool CartesianTrajectoryController::starting()
{
// time
- last_time_ = robot_state_->hw_->current_time_;
+ last_time_ = robot_state_->getTime();
// set desired pose to current pose
pose_current_ = getPose();
@@ -204,7 +204,7 @@
void CartesianTrajectoryController::update()
{
// get time
- double time = robot_state_->hw_->current_time_;
+ double time = robot_state_->getTime();
double dt = time - last_time_;
last_time_ = time;
Modified: pkg/trunk/sandbox/experimental_controllers/src/cartesian_twist_controller_ik.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/cartesian_twist_controller_ik.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/cartesian_twist_controller_ik.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -142,7 +142,7 @@
fb_pid_controller_[i].reset();
// time
- last_time_ = robot_state_->hw_->current_time_;
+ last_time_ = robot_state_->getTime();
// set disired twist to 0
twist_desi_ = Twist::Zero();
@@ -157,7 +157,7 @@
if (!chain_.allCalibrated(robot_state_->joint_states_)) return;
// get time
- double time = robot_state_->hw_->current_time_;
+ double time = robot_state_->getTime();
double dt = time - last_time_;
last_time_ = time;
Modified: pkg/trunk/sandbox/experimental_controllers/src/endeffector_constraint_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/endeffector_constraint_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/endeffector_constraint_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -34,7 +34,6 @@
#include "experimental_controllers/endeffector_constraint_controller.h"
#include <algorithm>
#include "angles/angles.h"
-#include "urdf/parser.h"
#include "kdl/chainfksolverpos_recursive.hpp"
#include "kdl/chainjnttojacsolver.hpp"
Modified: pkg/trunk/sandbox/experimental_controllers/src/joint_autotuner.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/joint_autotuner.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/joint_autotuner.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -75,7 +75,7 @@
assert(robot);
robot_ = robot->model_;
robot_state_ = robot;
- last_time_ = robot->hw_->current_time_;
+ last_time_ = robot->getTime();
cycle_start_time_ = getTime();
TiXmlElement *j = config->FirstChildElement("joint");
if (!j)
@@ -148,7 +148,7 @@
double JointAutotuner::getTime()
{
- return robot_state_->hw_->current_time_;
+ return robot_state_->getTime();
}
void JointAutotuner::update()
Modified: pkg/trunk/sandbox/experimental_controllers/src/joint_calibration_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/joint_calibration_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/joint_calibration_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -191,12 +191,12 @@
if (c_.calibrated())
{
- if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ if (last_publish_time_ + 0.5 < robot_->getTime())
{
assert(pub_calibrated_);
if (pub_calibrated_->trylock())
{
- last_publish_time_ = robot_->hw_->current_time_;
+ last_publish_time_ = robot_->getTime();
pub_calibrated_->unlockAndPublish();
}
}
Modified: pkg/trunk/sandbox/experimental_controllers/src/joint_chain_constraint_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/joint_chain_constraint_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/joint_chain_constraint_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -95,7 +95,7 @@
task_torque_ = Eigen::MatrixXf::Zero(kdl_chain_.getNrOfJoints(), 1);
identity_ = Eigen::MatrixXf::Identity(kdl_chain_.getNrOfJoints(), kdl_chain_.getNrOfJoints());
chain_eigen_jacobian_ = Eigen::MatrixXf::Zero(6, kdl_chain_.getNrOfJoints());
- last_time_ = robot_state->hw_->current_time_;
+ last_time_ = robot_state->getTime();
// advertise service
node_->advertiseService(controller_name_ + "/add_constraints", &JointChainConstraintController::addConstraint, this);
@@ -162,7 +162,7 @@
return;
}
- double time = robot_state_->hw_->current_time_;
+ double time = robot_state_->getTime();
double error(0);
std::list<ConstraintState>::iterator it = constraint_list_.begin();
for(int i = 0 ; i < list_size_; ++i)
Modified: pkg/trunk/sandbox/experimental_controllers/src/joint_chain_sine_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/joint_chain_sine_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/joint_chain_sine_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -73,7 +73,7 @@
robot_state_ = robot_state;
// initialize the time
- last_time_ = robot_state_->hw_->current_time_;
+ last_time_ = robot_state_->getTime();
// get the controller name
controller_name_ = config->Attribute("name");
@@ -236,7 +236,7 @@
}
// update the time
- double time = robot_state_->hw_->current_time_;
+ double time = robot_state_->getTime();
double dt = time - last_time_;
// calculate the desired positions and velocities for all joints:
Modified: pkg/trunk/sandbox/experimental_controllers/src/joint_inverse_dynamics_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/joint_inverse_dynamics_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/joint_inverse_dynamics_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -153,9 +153,7 @@
{
// set desired wrench to 0
SetToZero(jnt_posvelacc_desi_);
-
- last_time_ = robot_state_->hw_->current_time_;
-
+ last_time_ = robot_state_->getTime();
return true;
}
Modified: pkg/trunk/sandbox/experimental_controllers/src/joint_limit_calibration_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/joint_limit_calibration_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/joint_limit_calibration_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -195,12 +195,12 @@
if (c_.calibrated())
{
- if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ if (last_publish_time_ + 0.5 < robot_->getTime())
{
assert(pub_calibrated_);
if (pub_calibrated_->trylock())
{
- last_publish_time_ = robot_->hw_->current_time_;
+ last_publish_time_ = robot_->getTime();
pub_calibrated_->unlockAndPublish();
}
}
Modified: pkg/trunk/sandbox/experimental_controllers/src/joint_pd_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/joint_pd_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/joint_pd_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -62,7 +62,7 @@
{
assert(robot);
robot_ = robot;
- last_time_ = robot->hw_->current_time_;
+ last_time_ = robot->getTime();
joint_state_ = robot_->getJointState(joint_name);
if (!joint_state_)
@@ -146,7 +146,7 @@
void JointPDController::update()
{
double error(0), error_dot(0);
- double time = robot_->hw_->current_time_;
+ double time = robot_->getTime();
if(pthread_mutex_trylock(&joint_pd_controller_lock_) == 0)
{
Modified: pkg/trunk/sandbox/experimental_controllers/src/joint_position_smoothing_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/joint_position_smoothing_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/joint_position_smoothing_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -55,7 +55,7 @@
{
assert(robot);
robot_ = robot;
- last_time_ = robot->hw_->current_time_;
+ last_time_ = robot->getTime();
joint_state_ = robot_->getJointState(joint_name);
if (!joint_state_)
@@ -139,7 +139,7 @@
assert(robot_ != NULL);
double error(0);
- double time = robot_->hw_->current_time_;
+ double time = robot_->getTime();
assert(joint_state_->joint_);
Modified: pkg/trunk/sandbox/experimental_controllers/src/joint_trajectory_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/joint_trajectory_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/joint_trajectory_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -134,7 +134,7 @@
bool JointTrajectoryController::starting()
{
- current_time_ = robot_->hw_->current_time_;
+ current_time_ = robot_state_->getTime();
updateJointValues();
last_time_ = current_time_;
setTrajectoryCmdToCurrentValues();
@@ -165,7 +165,7 @@
delete diagnostics_publisher_ ;
diagnostics_publisher_ = new realtime_tools::RealtimePublisher <diagnostic_msgs::DiagnosticArray> ("/diagnostics", 2) ;
- last_diagnostics_publish_time_ = robot_->hw_->current_time_;
+ last_diagnostics_publish_time_ = robot_state_->getTime();
node_->param<double>(prefix_+"diagnostics_publish_delta_time",diagnostics_publish_delta_time_,1.0);
controller_state_publisher_->msg_.name = name_;
@@ -215,7 +215,7 @@
robot_ = robot->model_;
robot_state_ = robot;
- current_time_ = robot_->hw_->current_time_;
+ current_time_ = robot_state_->getTime();
TiXmlElement *elt = config->FirstChildElement("controller");
while (elt)
{
@@ -463,7 +463,7 @@
#ifdef PUBLISH_MAX_TIME
double start_time = ros::Time::now().toSec();
#endif
- current_time_ = robot_->hw_->current_time_;
+ current_time_ = robot_state_->getTime();
updateJointValues();
int get_trajectory_index = 0;
Modified: pkg/trunk/sandbox/experimental_controllers/src/pid_position_velocity_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/pid_position_velocity_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/pid_position_velocity_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -61,7 +61,7 @@
{
assert(robot);
robot_ = robot;
- last_time_ = robot->hw_->current_time_;
+ last_time_ = robot->getTime();
joint_state_ = robot_->getJointState(joint_name);
if (!joint_state_)
@@ -140,7 +140,7 @@
{
assert(robot_ != NULL);
double error(0),error_dot(0);
- double time = robot_->hw_->current_time_;
+ double time = robot_->getTime();
error_dot = joint_state_->velocity_ - command_dot_;
Modified: pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -115,7 +115,7 @@
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_;
+ last_time_ = robot->model_->getTime();
// Constructs solvers and allocates matrices.
@@ -204,7 +204,7 @@
void PlugController::computeConstraintJacobian()
{
- double time = robot_->model_->hw_->current_time_;
+ double time = robot_->model_->getTime();
// Clear force vector
f_r_ = 0;
f_roll_ = 0;
Modified: pkg/trunk/sandbox/experimental_controllers/src/pr2_gripper_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/pr2_gripper_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/pr2_gripper_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -146,7 +146,7 @@
return; // motor's not calibrated
}
- double current_time = robot_state_->hw_->current_time_;
+ double current_time = robot_state_->getTime();
if(new_cmd_available_)
{
if(pthread_mutex_trylock(&pr2_gripper_controller_lock_) == 0) //the callback is not writing to grasp_cmd_
@@ -246,8 +246,8 @@
bool Pr2GripperController::starting()
{
- last_time_ = robot_state_->hw_->current_time_;
- cmd_received_timestamp_ = robot_state_->hw_->current_time_;
+ last_time_ = robot_state_->getTime();
+ cmd_received_timestamp_ = robot_state_->getTime();
return true;
}
@@ -264,7 +264,7 @@
double Pr2GripperController::rampMove(double start_force, double end_force, double time, double hold)
{
double del_force = end_force - start_force;
- double del_time = robot_state_->hw_->current_time_ - cmd_received_timestamp_;
+ double del_time = robot_state_->getTime() - cmd_received_timestamp_;
if(del_time > time)
return hold;
return start_force + del_time/time*del_force;
@@ -309,7 +309,7 @@
//peak_force_second_grasp = 0;
peak_distance_first_grasp = 0.0;
//peak_distance_second_grasp = 0.0;
- grasp_open_close_timestamp = robot_state_->hw_->current_time_;
+ grasp_open_close_timestamp = robot_state_->getTime();
closed_loop_grasp_state = open0;
velocity_mode_ = false;
if(service_callback && closed_loop_trail_ > 0)
@@ -321,7 +321,7 @@
else if(closed_loop_grasp_state == open0)
{
//check for any timeouts
- if(grasp_open_close_timestamp + timeout_duration_ < robot_state_->hw_->current_time_)
+ if(grasp_open_close_timestamp + timeout_duration_ < robot_state_->getTime())
{
ROS_WARN("grasp failed due to a timeout");
closed_loop_grasp_state = failed;
@@ -348,7 +348,7 @@
if (service_callback_ && closed_loop_trail_ > 0 && joint_->position_ - .01 > service_response_.distance[0]) //if it's outside of the gripper
{
- service_response_.time_to_return[closed_loop_trail_ - 1] = robot_state_->hw_->current_time_ - grasp_open_close_timestamp;
+ service_response_.time_to_return[closed_loop_trail_ - 1] = robot_state_->getTime() - grasp_open_close_timestamp;
//read the gripper pads to zero them
for(int i = 0; i < num_pressure_pads_front_; i++) //the front pads are 7-21
{
@@ -362,12 +362,12 @@
}
//chage state
closed_loop_grasp_state = close0_closing;
- grasp_open_close_timestamp = robot_state_->hw_->current_time_;
+ grasp_open_close_timestamp = robot_state_->getTime();
velocity_mode_ = true;
return -1.0*default_low_speed_;
}
//if the gripper is done opening
- else if(joint_->velocity_ < stopped_threshold_ && joint_->velocity_ > -1.0*stopped_threshold_ && grasp_open_close_timestamp + .1 < robot_state_->hw_->current_time_)
+ else if(joint_->velocity_ < stopped_threshold_ && joint_->velocity_ > -1.0*stopped_threshold_ && grasp_open_close_timestamp + .1 < robot_state_->getTime())
{
if(service_callback_ && closed_loop_trail_ > 0)
{
@@ -392,7 +392,7 @@
}
//chage state
closed_loop_grasp_state = close0_closing;
- grasp_open_close_timestamp = robot_state_->hw_->current_time_;
+ grasp_open_close_timestamp = robot_state_->getTime();
velocity_mode_ = true;
return -1.0*default_low_speed_;
}
@@ -478,7 +478,7 @@
return default_effort_;
}
//check for any timeouts
- if(grasp_open_close_timestamp + timeout_duration_ < robot_state_->hw_->current_time_)
+ if(grasp_open_close_timestamp + timeout_duration_ < robot_state_->getTime())
{
ROS_WARN("grasp failed due to a timeout");
closed_loop_grasp_state = failed;
@@ -507,7 +507,7 @@
{
position_first_contact = joint_->position_;
closed_loop_grasp_state = close0_contact;
- grasp_open_close_timestamp = robot_state_->hw_->current_time_;
+ grasp_open_close_timestamp = robot_state_->getTime();
if(closed_loop && service_callback && joint_->position_ > service_request_.distance + service_request_.distance_tolerance)
{
ROS_WARN("grasp failed due to closing too little before impacting object");
@@ -561,8 +561,8 @@
//record time to first steady state info
if(service_callback && closed_loop_trail_ > 0 && service_response_.time_to_first[closed_loop_trail_-1] < 0.0 && joint_->position_ < service_response_.distance_compressed[0])
{
- service_response_.time_to_first[closed_loop_trail_ - 1] = robot_state_->hw_->current_time_- grasp_open_close_timestamp;
- ROS_WARN("Assigning time to first %i to be %f", closed_loop_trail_-1, robot_state_->hw_->current_time_- grasp_open_close_timestamp);
+ service_response_.time_to_first[closed_loop_trail_ - 1] = robot_state_->getTime()- grasp_open_close_timestamp;
+ ROS_WARN("Assigning time to first %i to be %f", closed_loop_trail_-1, robot_state_->getTime()- grasp_open_close_timestamp);
}
//record final contact info
@@ -603,7 +603,7 @@
service_response_.fingertip_profile0[i] = pressure_state_.data0[i]-fingertip_sensor_sides_start0_[i];
service_response_.fingertip_profile1[i] = pressure_state_.data1[i]-fingertip_sensor_sides_start1_[i];
}
- service_response_.time[closed_loop_trail_] = robot_state_->hw_->current_time_- grasp_open_close_timestamp;
+ service_response_.time[closed_loop_trail_] = robot_state_->getTime()- grasp_open_close_timestamp;
service_response_.force_peak0[closed_loop_trail_] = force_peak0;
service_response_.force_steady0[closed_loop_trail_] = force_steady0;
service_response_.force_peak1[closed_loop_trail_] = force_peak1;
@@ -639,11 +639,11 @@
else
{
//closed_loop_grasp_state = open1;
- //grasp_open_close_timestamp = robot_state_->hw_->current_time_;
+ //grasp_open_close_timestamp = robot_state_->getTime();
closed_loop_grasp_state = complete;
}
}
- else if(grasp_open_close_timestamp + timeout_duration_steady_ < robot_state_->hw_->current_time_)
+ else if(grasp_open_close_timestamp + timeout_duration_steady_ < robot_state_->getTime())
{
ROS_WARN("grasp failed due to a timeout");
closed_loop_grasp_state = failed;
@@ -673,10 +673,10 @@
else if(closed_loop_grasp_state == open1)
{
//if I'm done opening
- if(robot_state_->hw_->current_time_ > grasp_open_close_timestamp.toSec() + break_stiction_period_)
+ if(robot_state_->getTime() > grasp_open_close_timestamp.toSec() + break_stiction_period_)
{
closed_loop_grasp_state = close1_closing;
- grasp_open_close_timestamp = robot_state_->hw_->current_time_;
+ grasp_open_close_timestamp = robot_state_->getTime();
velocity_mode_ = true;
return -1.0*default_high_speed_;
}
@@ -702,7 +702,7 @@
starting_force_sum += fingertip_sensor_start1_[i];
}
//check for any timeouts
- if(grasp_open_close_timestamp + timeout_duration_ < robot_state_->hw_->current_time_)
+ if(grasp_open_close_timestamp + timeout_duration_ < robot_state_->getTime())
{
ROS_WARN("grasp failed due to a timeout");
closed_loop_grasp_state = failed;
@@ -745,7 +745,7 @@
{
position_second_contact = joint_->position_;
closed_loop_grasp_state = close1_contact;
- grasp_open_close_timestamp = robot_state_->hw_->current_time_;
+ grasp_open_close_timestamp = robot_state_->getTime();
}
velocity_mode_ = true;
return -1.0*default_high_speed_;
@@ -774,7 +774,7 @@
}
}
//record final contact info
- if(grasp_open_close_timestamp + timeout_duration_steady_ < robot_state_->hw_->current_time_)
+ if(grasp_open_close_timestamp + timeout_duration_steady_ < robot_state_->getTime())
{
for(int i = 0; i < num_pressure_pads_front_; i++)
{
@@ -783,7 +783,7 @@
}
position_second_compression = joint_->position_;
closed_loop_grasp_state = complete;
- grasp_open_close_timestamp = robot_state_->hw_->current_time_;
+ grasp_open_close_timestamp = robot_state_->getTime();
//compute k
spring_const = (high_force_-low_force_)/(position_second_compression - position_first_compression);
@@ -1020,7 +1020,7 @@
grasp_cmd_.time = grasp_cmd->time;
grasp_cmd_.val = grasp_cmd->val;
closed_loop_trail_ = 0;
- cmd_received_timestamp_ = robot_state_->hw_->current_time_;
+ cmd_received_timestamp_ = robot_state_->getTime();
new_cmd_available_ = true;
pthread_mutex_unlock(&pr2_gripper_controller_lock_);
}
@@ -1048,7 +1048,7 @@
closed_loop_trail_ = 0;
if(req.trials < 1)
return false;
- cmd_received_timestamp_ = robot_state_->hw_->current_time_;
+ cmd_received_timestamp_ = robot_state_->getTime();
service_response_.fingertip_profile0.resize(num_pressure_pads_front_+num_pressure_pads_side_);
service_response_.fingertip_profile1.resize(num_pressure_pads_front_+num_pressure_pads_side_);
service_response_.distance.resize(req.trials);
Modified: pkg/trunk/sandbox/experimental_controllers/src/probe.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/probe.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/probe.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -75,7 +75,7 @@
msg.velocity = joint_->velocity_;
msg.torque = joint_->commanded_effort_;
msg.torque_measured = joint_->applied_effort_;
- msg.time_step = robot_->hw_->current_time_;
+ msg.time_step = robot_->getTime();
pub_probe_.publish(msg);
}
Modified: pkg/trunk/sandbox/experimental_controllers/src/trajectory_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/trajectory_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/src/trajectory_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -137,7 +137,7 @@
new_cmd_available_ = false;
trajectory_status_ = manipulation_srvs::QuerySplineTraj::Response::State_Done;
spline_done_ = false;
- last_time_ = robot_state_->hw_->current_time_;
+ last_time_ = robot_state_->getTime();
goal_ = getCommand(spline_rt_.segments.back(), spline_rt_.segments.back().duration.toSec());
spline_time_ = spline_rt_.segments.back().duration.toSec();
@@ -147,7 +147,7 @@
void TrajectoryController::update()
{
- spline_time_ += (robot_state_->hw_->current_time_ - last_time_);
+ spline_time_ += (robot_state_->getTime() - last_time_);
if(new_cmd_available_)
{
@@ -189,7 +189,7 @@
setCommand(joint_cmd);
- last_time_ = robot_state_->hw_->current_time_;
+ last_time_ = robot_state_->getTime();
}
manipulation_msgs::Waypoint TrajectoryController::getCommand(const manipulation_msgs::SplineTrajSegment &spline, const double t)
Modified: pkg/trunk/stacks/mechanism/kdl...
[truncated message content] |
