Program Listing for File offsets.hpp

↰ Return to documentation for file (/tmp/ws/src/robot_calibration/robot_calibration/include/robot_calibration/optimization/offsets.hpp)

/*
 * Copyright (C) 2019 Michael Ferguson
 * Copyright (C) 2013-2014 Unbounded Robotics Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

// Author: Michael Ferguson

#ifndef ROBOT_CALIBRATION_OPTIMIZATION_OFFSETS_HPP
#define ROBOT_CALIBRATION_OPTIMIZATION_OFFSETS_HPP

#include <kdl/chain.hpp>

namespace robot_calibration
{

class OptimizationOffsets
{
public:
  OptimizationOffsets();
  virtual ~OptimizationOffsets() {}

  bool add(const std::string name);

  bool addFrame(const std::string name,
                bool calibrate_x, bool calibrate_y, bool calibrate_z,
                bool calibrate_roll, bool calibrate_pitch, bool calibrate_yaw);

  bool set(const std::string name, double value);

  bool setFrame(const std::string name,
                double x, double y, double z,
                double roll, double pitch, double yaw);

  bool initialize(double* free_params);

  bool update(const double* const free_params);

  double get(const std::string name) const;

  bool getFrame(const std::string name, KDL::Frame& offset) const;

  size_t size();

  bool reset();

  bool loadOffsetYAML(const std::string& filename);

  std::string getOffsetYAML();

  std::string updateURDF(const std::string& urdf);

private:
  // Names of parameters being calibrated. The order of this vector
  // is the same as the free_param order will be interpreted.
  std::vector<std::string> parameter_names_;

  // Names of frames being calibrated.
  std::vector<std::string> frame_names_;

  // Values of parameters from last update
  std::vector<double> parameter_offsets_;

  // Number of params being calibrated
  size_t num_free_params_;

  // No copy
  OptimizationOffsets(const OptimizationOffsets&);
  OptimizationOffsets& operator=(const OptimizationOffsets&);
};

}  // namespace robot_calibration

#endif  // ROBOT_CALIBRATION_OPTIMIZATION_OFFSETS_HPP