Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <vector>
00019 #include <string>
00020
00021 #include <ros/ros.h>
00022 #include <std_msgs/Float64.h>
00023 #include <trajectory_msgs/JointTrajectory.h>
00024 #include <kdl/jntarray.hpp>
00025 #include <cob_twist_controller/utils/simpson_integrator.h>
00026
00027 class TrajectoryCommandExecutionTester
00028 {
00029 public:
00030 TrajectoryCommandExecutionTester()
00031 {
00032 dof_ = 2;
00033 idx_ = 0;
00034
00035 q_.resize(dof_);
00036 KDL::SetToZero(q_);
00037 q_dot_.resize(dof_);
00038 KDL::SetToZero(q_dot_);
00039 simpson_q_.resize(dof_);
00040 KDL::SetToZero(simpson_q_);
00041 euler_q_.resize(dof_);
00042 KDL::SetToZero(euler_q_);
00043 derived_simpson_q_dot_.resize(dof_);
00044 KDL::SetToZero(derived_simpson_q_dot_);
00045
00046 integrator_.reset(new SimpsonIntegrator(dof_));
00047 trajectory_pub_ = nh_.advertise<trajectory_msgs::JointTrajectory>("joint_trajectory_controller/command", 1);
00048
00049 output_q_pub_ = nh_.advertise<std_msgs::Float64>("integrator_debug/q", 1);
00050 output_q_dot_pub_ = nh_.advertise<std_msgs::Float64>("integrator_debug/q_dot", 1);
00051 output_simpson_q_pub_ = nh_.advertise<std_msgs::Float64>("integrator_debug/simpson_q", 1);
00052 output_euler_q_pub_ = nh_.advertise<std_msgs::Float64>("integrator_debug/euler_q", 1);
00053 output_derived_simpson_q_dot_pub_ = nh_.advertise<std_msgs::Float64>("integrator_debug/derived_simpson_q_dot", 1);
00054
00055 ros::Duration(1.0).sleep();
00056 }
00057
00058
00059 ~TrajectoryCommandExecutionTester()
00060 {}
00061
00062 void run()
00063 {
00064 ros::Rate r(100.0);
00065
00066 ros::Time time = ros::Time::now();
00067 ros::Time last_update_time = time;
00068 ros::Duration period = time - last_update_time;
00069 ros::Time start_time = time;
00070 double x = time.toSec() - start_time.toSec();
00071 double old_pos = -99;
00072
00073
00074 double a = 0.6, b = 0.4, c = 0, d = 0;
00075 euler_q_(idx_) = a*sin(b*x+c) + d;
00076
00077 trajectory_msgs::JointTrajectoryPoint traj_point;
00078 traj_point.positions.assign(dof_, 0.0);
00079
00080
00081 std::vector<std::string> joint_names;
00082
00083
00084
00085
00086
00087
00088
00089 joint_names.push_back("torso_2_joint");
00090 joint_names.push_back("torso_3_joint");
00091
00092 while (ros::ok())
00093 {
00094 time = ros::Time::now();
00095 period = time - last_update_time;
00096 last_update_time = time;
00097 x = time.toSec() - start_time.toSec();
00098
00099 std::vector<double> next_q;
00100 std::vector<double> next_q_dot;
00101
00102 q_(idx_) = a*sin(b*x+c) + d;
00103 q_dot_(idx_) = a*b*cos(b*x+c);
00104
00105 if (integrator_->updateIntegration(q_dot_, q_, next_q, next_q_dot))
00106 {
00107 simpson_q_(idx_) = next_q[idx_];
00108
00109 }
00110
00111 euler_q_(idx_) += q_dot_(idx_) * period.toSec();
00112
00113 traj_point.positions[idx_] = q_(idx_);
00114
00115
00116 traj_point.time_from_start = ros::Duration(period.toSec());
00117
00119
00120 trajectory_msgs::JointTrajectory traj_msg;
00121 traj_msg.points.push_back(traj_point);
00122 traj_msg.joint_names = joint_names;
00123
00124
00125
00126 std_msgs::Float64 q_msg;
00127 q_msg.data = q_(idx_);
00128 std_msgs::Float64 q_dot_msg;
00129 q_dot_msg.data = q_dot_(idx_);
00130 std_msgs::Float64 simpson_q_msg;
00131 simpson_q_msg.data = simpson_q_(idx_);
00132 std_msgs::Float64 euler_q_msg;
00133 euler_q_msg.data = euler_q_(idx_);
00134 std_msgs::Float64 derived_simpson_q_dot_msg;
00135 derived_simpson_q_dot_msg.data = derived_simpson_q_dot_(idx_);
00136
00137 output_q_pub_.publish(q_msg);
00138 output_q_dot_pub_.publish(q_dot_msg);
00139 output_simpson_q_pub_.publish(simpson_q_msg);
00140 output_euler_q_pub_.publish(euler_q_msg);
00141 output_derived_simpson_q_dot_pub_.publish(derived_simpson_q_dot_msg);
00142
00143 trajectory_pub_.publish(traj_msg);
00144
00145 ros::spinOnce();
00146 r.sleep();
00147 }
00148 }
00149
00150
00151 ros::NodeHandle nh_;
00152 ros::Publisher trajectory_pub_;
00153
00154 ros::Publisher output_q_pub_;
00155 ros::Publisher output_q_dot_pub_;
00156 ros::Publisher output_simpson_q_pub_;
00157 ros::Publisher output_euler_q_pub_;
00158 ros::Publisher output_derived_simpson_q_dot_pub_;
00159
00160 KDL::JntArray q_;
00161 KDL::JntArray q_dot_;
00162 KDL::JntArray simpson_q_;
00163 KDL::JntArray euler_q_;
00164 KDL::JntArray derived_simpson_q_dot_;
00165
00166 boost::shared_ptr<SimpsonIntegrator> integrator_;
00167
00168 unsigned int dof_;
00169 unsigned int idx_;
00170 };
00171
00172
00173
00174 int main(int argc, char **argv)
00175 {
00176 ros::init(argc, argv, "test_trajectory_command_execution_node");
00177
00178 TrajectoryCommandExecutionTester tcet;
00179 tcet.run();
00180 ros::spin();
00181 return 0;
00182 }