1 |
|
|
/* |
2 |
|
|
* Copyright 2011, |
3 |
|
|
* Olivier Stasse, |
4 |
|
|
* |
5 |
|
|
* CNRS |
6 |
|
|
* |
7 |
|
|
*/ |
8 |
|
|
/* -------------------------------------------------------------------------- */ |
9 |
|
|
/* --- INCLUDES ------------------------------------------------------------- */ |
10 |
|
|
/* -------------------------------------------------------------------------- */ |
11 |
|
|
|
12 |
|
|
#include <ros/rate.h> |
13 |
|
|
|
14 |
|
|
#include <dynamic_graph_bridge/sot_loader.hh> |
15 |
|
|
|
16 |
|
|
#include "dynamic_graph_bridge/ros_init.hh" |
17 |
|
|
|
18 |
|
|
// POSIX.1-2001 |
19 |
|
|
#include <dlfcn.h> |
20 |
|
|
|
21 |
|
|
using namespace std; |
22 |
|
|
using namespace dynamicgraph::sot; |
23 |
|
|
namespace po = boost::program_options; |
24 |
|
|
|
25 |
|
|
struct DataToLog { |
26 |
|
|
const std::size_t N; |
27 |
|
|
std::size_t idx, iter; |
28 |
|
|
|
29 |
|
|
std::vector<std::size_t> iters; |
30 |
|
|
std::vector<double> times; |
31 |
|
|
std::vector<double> ittimes; |
32 |
|
|
|
33 |
|
|
DataToLog(std::size_t N_) |
34 |
|
|
: N(N_), idx(0), iter(0), iters(N, 0), times(N, 0), ittimes(N, 0) {} |
35 |
|
|
|
36 |
|
|
void record(const double t, const double itt) { |
37 |
|
|
iters[idx] = iter; |
38 |
|
|
times[idx] = t; |
39 |
|
|
ittimes[idx] = itt; |
40 |
|
|
++idx; |
41 |
|
|
++iter; |
42 |
|
|
if (idx == N) idx = 0; |
43 |
|
|
} |
44 |
|
|
|
45 |
|
|
void save(const char *prefix) { |
46 |
|
|
std::ostringstream oss; |
47 |
|
|
oss << prefix << "-times.log"; |
48 |
|
|
|
49 |
|
|
std::ofstream aof(oss.str().c_str()); |
50 |
|
|
if (aof.is_open()) { |
51 |
|
|
for (std::size_t k = 0; k < N; ++k) { |
52 |
|
|
aof << iters[(idx + k) % N] << ' ' << times[(idx + k) % N] << ' ' |
53 |
|
|
<< ittimes[(idx + k) % N] << '\n'; |
54 |
|
|
} |
55 |
|
|
} |
56 |
|
|
aof.close(); |
57 |
|
|
} |
58 |
|
|
}; |
59 |
|
|
|
60 |
|
|
void workThreadLoader(SotLoader *aSotLoader) { |
61 |
|
|
ros::Rate rate(1000); // 1 kHz |
62 |
|
|
if (ros::param::has("/sot_controller/dt")) { |
63 |
|
|
double periodd; |
64 |
|
|
ros::param::get("/sot_controller/dt", periodd); |
65 |
|
|
rate = ros::Rate(1 / periodd); |
66 |
|
|
} |
67 |
|
|
DataToLog dataToLog(5000); |
68 |
|
|
|
69 |
|
|
rate.reset(); |
70 |
|
|
while (ros::ok() && aSotLoader->isDynamicGraphStopped()) { |
71 |
|
|
rate.sleep(); |
72 |
|
|
} |
73 |
|
|
|
74 |
|
|
ros::NodeHandle nh("/geometric_simu"); |
75 |
|
|
bool paused; |
76 |
|
|
ros::Time timeOrigin = ros::Time::now(); |
77 |
|
|
ros::Time time; |
78 |
|
|
while (ros::ok() && !aSotLoader->isDynamicGraphStopped()) { |
79 |
|
|
nh.param<bool>("paused", paused, false); |
80 |
|
|
|
81 |
|
|
if (!paused) { |
82 |
|
|
time = ros::Time::now(); |
83 |
|
|
aSotLoader->oneIteration(); |
84 |
|
|
|
85 |
|
|
ros::Duration d = ros::Time::now() - time; |
86 |
|
|
dataToLog.record((time - timeOrigin).toSec(), d.toSec()); |
87 |
|
|
} |
88 |
|
|
rate.sleep(); |
89 |
|
|
} |
90 |
|
|
dataToLog.save("/tmp/geometric_simu"); |
91 |
|
|
ros::waitForShutdown(); |
92 |
|
|
} |
93 |
|
|
|
94 |
|
|
SotLoader::SotLoader() |
95 |
|
|
: sensorsIn_(), |
96 |
|
|
controlValues_(), |
97 |
|
|
angleEncoder_(), |
98 |
|
|
angleControl_(), |
99 |
|
|
forces_(), |
100 |
|
|
torques_(), |
101 |
|
|
baseAtt_(), |
102 |
|
|
accelerometer_(3), |
103 |
|
|
gyrometer_(3), |
104 |
|
|
thread_() { |
105 |
|
|
readSotVectorStateParam(); |
106 |
|
|
initPublication(); |
107 |
|
|
|
108 |
|
|
freeFlyerPose_.header.frame_id = "odom"; |
109 |
|
|
freeFlyerPose_.child_frame_id = "base_link"; |
110 |
|
|
if (ros::param::get("/sot/tf_base_link", freeFlyerPose_.child_frame_id)) { |
111 |
|
|
ROS_INFO_STREAM("Publishing " << freeFlyerPose_.child_frame_id << " wrt " |
112 |
|
|
<< freeFlyerPose_.header.frame_id); |
113 |
|
|
} |
114 |
|
|
} |
115 |
|
|
|
116 |
|
|
SotLoader::~SotLoader() { |
117 |
|
|
dynamic_graph_stopped_ = true; |
118 |
|
|
thread_.join(); |
119 |
|
|
} |
120 |
|
|
|
121 |
|
|
void SotLoader::startControlLoop() { |
122 |
|
|
thread_ = boost::thread(workThreadLoader, this); |
123 |
|
|
} |
124 |
|
|
|
125 |
|
|
void SotLoader::initializeRosNode(int argc, char *argv[]) { |
126 |
|
|
SotLoaderBasic::initializeRosNode(argc, argv); |
127 |
|
|
// Temporary fix. TODO: where should nbOfJoints_ be initialized from? |
128 |
|
|
if (ros::param::has("/sot/state_vector_map")) { |
129 |
|
|
angleEncoder_.resize(nbOfJoints_); |
130 |
|
|
angleControl_.resize(nbOfJoints_); |
131 |
|
|
} |
132 |
|
|
|
133 |
|
|
startControlLoop(); |
134 |
|
|
} |
135 |
|
|
|
136 |
|
|
void SotLoader::fillSensors(map<string, dgs::SensorValues> &sensorsIn) { |
137 |
|
|
// Update joint values.w |
138 |
|
|
assert(angleControl_.size() == angleEncoder_.size()); |
139 |
|
|
|
140 |
|
|
sensorsIn["joints"].setName("angle"); |
141 |
|
|
for (unsigned int i = 0; i < angleControl_.size(); i++) |
142 |
|
|
angleEncoder_[i] = angleControl_[i]; |
143 |
|
|
sensorsIn["joints"].setValues(angleEncoder_); |
144 |
|
|
} |
145 |
|
|
|
146 |
|
|
void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) { |
147 |
|
|
// Update joint values. |
148 |
|
|
angleControl_ = controlValues["control"].getValues(); |
149 |
|
|
|
150 |
|
|
// Debug |
151 |
|
|
std::map<std::string, dgs::ControlValues>::iterator it = |
152 |
|
|
controlValues.begin(); |
153 |
|
|
sotDEBUG(30) << "ControlValues to be broadcasted:" << std::endl; |
154 |
|
|
for (; it != controlValues.end(); it++) { |
155 |
|
|
sotDEBUG(30) << it->first << ":"; |
156 |
|
|
std::vector<double> ctrlValues_ = it->second.getValues(); |
157 |
|
|
std::vector<double>::iterator it_d = ctrlValues_.begin(); |
158 |
|
|
for (; it_d != ctrlValues_.end(); it_d++) sotDEBUG(30) << *it_d << " "; |
159 |
|
|
sotDEBUG(30) << std::endl; |
160 |
|
|
} |
161 |
|
|
sotDEBUG(30) << "End ControlValues" << std::endl; |
162 |
|
|
|
163 |
|
|
// Check if the size if coherent with the robot description. |
164 |
|
|
if (angleControl_.size() != (unsigned int)nbOfJoints_) { |
165 |
|
|
std::cerr << " angleControl_" << angleControl_.size() << " and nbOfJoints" |
166 |
|
|
<< (unsigned int)nbOfJoints_ << " are different !" << std::endl; |
167 |
|
|
exit(-1); |
168 |
|
|
} |
169 |
|
|
// Publish the data. |
170 |
|
|
joint_state_.header.stamp = ros::Time::now(); |
171 |
|
|
for (int i = 0; i < nbOfJoints_; i++) { |
172 |
|
|
joint_state_.position[i] = angleControl_[i]; |
173 |
|
|
} |
174 |
|
|
for (unsigned int i = 0; i < parallel_joints_to_state_vector_.size(); i++) { |
175 |
|
|
joint_state_.position[i + nbOfJoints_] = |
176 |
|
|
coefficient_parallel_joints_[i] * |
177 |
|
|
angleControl_[parallel_joints_to_state_vector_[i]]; |
178 |
|
|
} |
179 |
|
|
|
180 |
|
|
joint_pub_.publish(joint_state_); |
181 |
|
|
|
182 |
|
|
// Publish robot pose |
183 |
|
|
// get the robot pose values |
184 |
|
|
std::vector<double> poseValue_ = controlValues["baseff"].getValues(); |
185 |
|
|
|
186 |
|
|
freeFlyerPose_.transform.translation.x = poseValue_[0]; |
187 |
|
|
freeFlyerPose_.transform.translation.y = poseValue_[1]; |
188 |
|
|
freeFlyerPose_.transform.translation.z = poseValue_[2]; |
189 |
|
|
|
190 |
|
|
freeFlyerPose_.transform.rotation.w = poseValue_[3]; |
191 |
|
|
freeFlyerPose_.transform.rotation.x = poseValue_[4]; |
192 |
|
|
freeFlyerPose_.transform.rotation.y = poseValue_[5]; |
193 |
|
|
freeFlyerPose_.transform.rotation.z = poseValue_[6]; |
194 |
|
|
|
195 |
|
|
freeFlyerPose_.header.stamp = joint_state_.header.stamp; |
196 |
|
|
// Publish |
197 |
|
|
freeFlyerPublisher_.sendTransform(freeFlyerPose_); |
198 |
|
|
} |
199 |
|
|
|
200 |
|
|
void SotLoader::setup() { |
201 |
|
|
fillSensors(sensorsIn_); |
202 |
|
|
sotController_->setupSetSensors(sensorsIn_); |
203 |
|
|
sotController_->getControl(controlValues_); |
204 |
|
|
readControl(controlValues_); |
205 |
|
|
} |
206 |
|
|
|
207 |
|
|
void SotLoader::oneIteration() { |
208 |
|
|
fillSensors(sensorsIn_); |
209 |
|
|
try { |
210 |
|
|
sotController_->nominalSetSensors(sensorsIn_); |
211 |
|
|
sotController_->getControl(controlValues_); |
212 |
|
|
} catch (std::exception &) { |
213 |
|
|
throw; |
214 |
|
|
} |
215 |
|
|
|
216 |
|
|
readControl(controlValues_); |
217 |
|
|
} |