towr_ros_interface.cc
Go to the documentation of this file.
1 /******************************************************************************
2 Copyright (c) 2018, Alexander W. Winkler. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without
5 modification, are permitted provided that the following conditions are met:
6 
7 * Redistributions of source code must retain the above copyright notice, this
8  list of conditions and the following disclaimer.
9 
10 * Redistributions in binary form must reproduce the above copyright notice,
11  this list of conditions and the following disclaimer in the documentation
12  and/or other materials provided with the distribution.
13 
14 * Neither the name of the copyright holder nor the names of its
15  contributors may be used to endorse or promote products derived from
16  this software without specific prior written permission.
17 
18 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 ******************************************************************************/
29 
31 
32 #include <std_msgs/Int32.h>
33 
34 #include <xpp_states/convert.h>
35 #include <xpp_msgs/topic_names.h>
36 #include <xpp_msgs/TerrainInfo.h>
37 
40 #include <towr_ros/topic_names.h>
42 
43 
44 namespace towr {
45 
46 
48 {
50 
53 
54  initial_state_pub_ = n.advertise<xpp_msgs::RobotStateCartesian>
56 
57  robot_parameters_pub_ = n.advertise<xpp_msgs::RobotParameters>
59 
60  solver_ = std::make_shared<ifopt::IpoptSolver>();
61 
62  visualization_dt_ = 0.01;
63 }
64 
67 {
68  BaseState goal;
69  goal.lin.at(kPos) = xpp::Convert::ToXpp(msg.goal_lin.pos);
70  goal.lin.at(kVel) = xpp::Convert::ToXpp(msg.goal_lin.vel);
71  goal.ang.at(kPos) = xpp::Convert::ToXpp(msg.goal_ang.pos);
72  goal.ang.at(kVel) = xpp::Convert::ToXpp(msg.goal_ang.vel);
73 
74  return goal;
75 }
76 
77 void
79 {
80  // robot model
81  formulation_.model_ = RobotModel(static_cast<RobotModel::Robot>(msg.robot));
82  auto robot_params_msg = BuildRobotParametersMsg(formulation_.model_);
83  robot_parameters_pub_.publish(robot_params_msg);
84 
85  // terrain
86  auto terrain_id = static_cast<HeightMap::TerrainID>(msg.terrain);
88 
89  int n_ee = formulation_.model_.kinematic_model_->GetNumberOfEndeffectors();
92 
94 
95  // solver parameters
96  SetIpoptParameters(msg);
97 
98  // visualization
100 
101  // Defaults to /home/user/.ros/
102  std::string bag_file = "towr_trajectory.bag";
103  if (msg.optimize || msg.play_initialization) {
104  nlp_ = ifopt::Problem();
105  for (auto c : formulation_.GetVariableSets(solution))
106  nlp_.AddVariableSet(c);
107  for (auto c : formulation_.GetConstraints(solution))
109  for (auto c : formulation_.GetCosts())
110  nlp_.AddCostSet(c);
111 
112  solver_->Solve(nlp_);
113  SaveOptimizationAsRosbag(bag_file, robot_params_msg, msg, false);
114  }
115 
116  // playback using terminal commands
117  if (msg.replay_trajectory || msg.play_initialization || msg.optimize) {
118  int success = system(("rosbag play --topics "
121  + " -r " + std::to_string(msg.replay_speed)
122  + " --quiet " + bag_file).c_str());
123  }
124 
125  if (msg.plot_trajectory) {
126  int success = system(("killall rqt_bag; rqt_bag " + bag_file + "&").c_str());
127  }
128 
129  // to publish entire trajectory (e.g. to send to controller)
130  // xpp_msgs::RobotStateCartesianTrajectory xpp_msg = xpp::Convert::ToRos(GetTrajectory());
131 }
132 
133 void
135 {
136  int n_ee = formulation_.initial_ee_W_.size();
140 
141  for (int ee_towr=0; ee_towr<n_ee; ++ee_towr) {
142  int ee_xpp = ToXppEndeffector(n_ee, ee_towr).first;
143  xpp.ee_contact_.at(ee_xpp) = true;
144  xpp.ee_motion_.at(ee_xpp).p_ = formulation_.initial_ee_W_.at(ee_towr);
145  xpp.ee_forces_.at(ee_xpp).setZero(); // zero for visualization
146  }
147 
149 }
150 
151 std::vector<TowrRosInterface::XppVec>
153 {
154  std::vector<XppVec> trajectories;
155 
156  for (int iter=0; iter<nlp_.GetIterationCount(); ++iter) {
157  nlp_.SetOptVariables(iter);
158  trajectories.push_back(GetTrajectory());
159  }
160 
161  return trajectories;
162 }
163 
166 {
167  XppVec trajectory;
168  double t = 0.0;
169  double T = solution.base_linear_->GetTotalTime();
170 
171  EulerConverter base_angular(solution.base_angular_);
172 
173  while (t<=T+1e-5) {
174  int n_ee = solution.ee_motion_.size();
175  xpp::RobotStateCartesian state(n_ee);
176 
177  state.base_.lin = ToXpp(solution.base_linear_->GetPoint(t));
178 
179  state.base_.ang.q = base_angular.GetQuaternionBaseToWorld(t);
180  state.base_.ang.w = base_angular.GetAngularVelocityInWorld(t);
181  state.base_.ang.wd = base_angular.GetAngularAccelerationInWorld(t);
182 
183  for (int ee_towr=0; ee_towr<n_ee; ++ee_towr) {
184  int ee_xpp = ToXppEndeffector(n_ee, ee_towr).first;
185 
186  state.ee_contact_.at(ee_xpp) = solution.phase_durations_.at(ee_towr)->IsContactPhase(t);
187  state.ee_motion_.at(ee_xpp) = ToXpp(solution.ee_motion_.at(ee_towr)->GetPoint(t));
188  state.ee_forces_ .at(ee_xpp) = solution.ee_force_.at(ee_towr)->GetPoint(t).p();
189  }
190 
191  state.t_global_ = t;
192  trajectory.push_back(state);
193  t += visualization_dt_;
194  }
195 
196  return trajectory;
197 }
198 
199 xpp_msgs::RobotParameters
201 {
202  xpp_msgs::RobotParameters params_msg;
203  auto max_dev_xyz = model.kinematic_model_->GetMaximumDeviationFromNominal();
204  params_msg.ee_max_dev = xpp::Convert::ToRos<geometry_msgs::Vector3>(max_dev_xyz);
205 
206  auto nominal_B = model.kinematic_model_->GetNominalStanceInBase();
207  int n_ee = nominal_B.size();
208  for (int ee_towr=0; ee_towr<n_ee; ++ee_towr) {
209  Vector3d pos = nominal_B.at(ee_towr);
210  params_msg.nominal_ee_pos.push_back(xpp::Convert::ToRos<geometry_msgs::Point>(pos));
211  params_msg.ee_names.push_back(ToXppEndeffector(n_ee, ee_towr).second);
212  }
213 
214  params_msg.base_mass = model.dynamic_model_->m();
215 
216  return params_msg;
217 }
218 
219 void
220 TowrRosInterface::SaveOptimizationAsRosbag (const std::string& bag_name,
221  const xpp_msgs::RobotParameters& robot_params,
222  const TowrCommandMsg user_command_msg,
223  bool include_iterations)
224 {
225  rosbag::Bag bag;
226  bag.open(bag_name, rosbag::bagmode::Write);
227  ::ros::Time t0(1e-6); // t=0.0 throws ROS exception
228 
229  // save the a-priori fixed optimization variables
230  bag.write(xpp_msgs::robot_parameters, t0, robot_params);
231  bag.write(towr_msgs::user_command+"_saved", t0, user_command_msg);
232 
233  // save the trajectory of each iteration
234  if (include_iterations) {
235  auto trajectories = GetIntermediateSolutions();
236  int n_iterations = trajectories.size();
237  for (int i=0; i<n_iterations; ++i)
238  SaveTrajectoryInRosbag(bag, trajectories.at(i), towr_msgs::nlp_iterations_name + std::to_string(i));
239 
240  // save number of iterations the optimizer took
241  std_msgs::Int32 m;
242  m.data = n_iterations;
244  }
245 
246  // save the final trajectory
247  auto final_trajectory = GetTrajectory();
249 
250  bag.close();
251 }
252 
253 void
255  const XppVec& traj,
256  const std::string& topic) const
257 {
258  for (const auto state : traj) {
259  auto timestamp = ::ros::Time(state.t_global_ + 1e-6); // t=0.0 throws ROS exception
260 
261  xpp_msgs::RobotStateCartesian msg;
262  msg = xpp::Convert::ToRos(state);
263  bag.write(topic, timestamp, msg);
264 
265  xpp_msgs::TerrainInfo terrain_msg;
266  for (auto ee : state.ee_motion_.ToImpl()) {
267  Vector3d n = formulation_.terrain_->GetNormalizedBasis(HeightMap::Normal, ee.p_.x(), ee.p_.y());
268  terrain_msg.surface_normals.push_back(xpp::Convert::ToRos<geometry_msgs::Vector3>(n));
269  terrain_msg.friction_coeff = formulation_.terrain_->GetFrictionCoeff();
270  }
271 
272  bag.write(xpp_msgs::terrain_info, timestamp, terrain_msg);
273  }
274 }
275 
276 } /* namespace towr */
277 
DynamicModel::Ptr dynamic_model_
Quaterniond q
void SetOptVariables(int iter)
int GetIterationCount() const
static xpp_msgs::StateLin3d ToRos(const StateLin3d &xpp)
towr_ros::TowrCommand TowrCommandMsg
void AddCostSet(CostTerm::Ptr cost_set)
void publish(const boost::shared_ptr< M > &message) const
bool & at(EndeffectorID ee)
void open(std::string const &filename, uint32_t mode=bagmode::Read)
static const std::string robot_state_desired("/xpp/state_des")
const VectorXd at(Dx deriv) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::vector< xpp::RobotStateCartesian > XppVec
std::vector< PhaseDurations::Ptr > phase_durations_
ContraintPtrVec GetConstraints(const SplineHolder &spline_holder) const
::ros::Publisher initial_state_pub_
::ros::Subscriber user_command_sub_
static const std::string nlp_iterations_count("/towr/nlp_iterations_count")
double visualization_dt_
duration between two rviz visualization states.
EndeffectorsMotion ee_motion_
virtual Parameters GetTowrParameters(int n_ee, const TowrCommandMsg &msg) const =0
Formulates the actual TOWR problem to be solved.
static HeightMap::Ptr MakeTerrain(TerrainID type)
ifopt::IpoptSolver::Ptr solver_
NLP solver, could also use SNOPT.
virtual void SetTowrInitialState()=0
Sets the base state and end-effector position.
static const std::string nlp_iterations_name("/towr/nlp_iterations_name")
geometry_msgs::TransformStamped t
void close()
ContraintPtrVec GetCosts() const
Vector3d GetAngularAccelerationInWorld(double t) const
StateAng3d ang
NodeSpline::Ptr base_linear_
void UserCommandCallback(const TowrCommandMsg &msg)
static std::pair< xpp::EndeffectorID, std::string > ToXppEndeffector(int number_of_ee, int towr_ee_id)
const VectorXd p() const
std::vector< NodeSpline::Ptr > ee_force_
Eigen::Quaterniond GetQuaternionBaseToWorld(double t) const
StateLin3d lin
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
HeightMap::Ptr terrain_
NodeSpline::Ptr base_angular_
virtual BaseState GetGoalState(const TowrCommandMsg &msg) const
Vector3d GetAngularVelocityInWorld(double t) const
::ros::Publisher robot_parameters_pub_
SplineHolder solution
the solution splines linked to the opt-variables.
xpp_msgs::RobotParameters BuildRobotParametersMsg(const RobotModel &model) const
VariablePtrVec GetVariableSets(SplineHolder &spline_holder)
ifopt::Problem nlp_
the actual nonlinear program to be solved.
std::vector< XppVec > GetIntermediateSolutions()
void AddConstraintSet(ConstraintSet::Ptr constraint_set)
static const std::string user_command("/towr/user_command")
static xpp::StateLinXd ToXpp(const towr::State &towr)
void SaveOptimizationAsRosbag(const std::string &bag_name, const xpp_msgs::RobotParameters &robot_params, const TowrCommandMsg user_command_msg, bool include_iterations=false)
void SaveTrajectoryInRosbag(rosbag::Bag &, const std::vector< xpp::RobotStateCartesian > &traj, const std::string &topic) const
KinematicModel::Ptr kinematic_model_
void AddVariableSet(VariableSet::Ptr variable_set)
void write(std::string const &topic, ros::MessageEvent< T > const &event)
static const std::string terrain_info("/xpp/terrain_info")
std::vector< NodeSpline::Ptr > ee_motion_
NlpFormulation formulation_
the default formulation, can be adapted
static const std::string robot_parameters("/xpp/params")
Endeffectors< Vector3d > ee_forces_
EndeffectorsContact ee_contact_
static StateLin3d ToXpp(const xpp_msgs::StateLin3d &ros)
virtual void SetIpoptParameters(const TowrCommandMsg &msg)=0
Sets the parameters of the nonlinear programming solver IPOPT.


towr_ros
Author(s): Alexander W. Winkler
autogenerated on Fri Apr 2 2021 02:14:21