Skip to content

Commit

Permalink
Format previous changes
Browse files Browse the repository at this point in the history
  • Loading branch information
stefanscherzinger committed Oct 18, 2024
1 parent 9ccf2e7 commit 9a2c5e1
Show file tree
Hide file tree
Showing 20 changed files with 22 additions and 57 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ class CartesianComplianceController : public cartesian_motion_controller::Cartes

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state) override;

controller_interface::return_type update(const rclcpp::Time & time,
const rclcpp::Duration & period) override;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,6 @@ CartesianComplianceController::on_init()
return TYPE::SUCCESS;
}


rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
CartesianComplianceController::on_configure(const rclcpp_lifecycle::State & previous_state)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -94,9 +94,9 @@ class DampedLeastSquaresSolver : public IKSolver
* \return True, if everything went well
*/

bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits,
const KDL::JntArray & lower_pos_limits) override;
bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh, const KDL::Chain & chain,
const KDL::JntArray & upper_pos_limits,
const KDL::JntArray & lower_pos_limits) override;

private:
std::shared_ptr<KDL::ChainJntToJacSolver> m_jnt_jacobian_solver;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -104,9 +104,9 @@ class ForwardDynamicsSolver : public IKSolver
* @return True, if everything went well
*/

bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits,
const KDL::JntArray & lower_pos_limits) override;
bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh, const KDL::Chain & chain,
const KDL::JntArray & upper_pos_limits,
const KDL::JntArray & lower_pos_limits) override;

private:
//! Build a generic robot model for control
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -148,9 +148,8 @@ class IKSolver
* @return True, if everything went well
*/

virtual bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits,
const KDL::JntArray & lower_pos_limits);
virtual bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh, const KDL::Chain & chain,
const KDL::JntArray & upper_pos_limits, const KDL::JntArray & lower_pos_limits);

/**
* @brief Update the robot kinematics of the solver
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -88,9 +88,9 @@ class JacobianTransposeSolver : public IKSolver
* \return True, if everything went well
*/

bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> /*nh*/,
const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits,
const KDL::JntArray & lower_pos_limits) override;
bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> /*nh*/, const KDL::Chain & chain,
const KDL::JntArray & upper_pos_limits,
const KDL::JntArray & lower_pos_limits) override;

private:
std::shared_ptr<KDL::ChainJntToJacSolver> m_jnt_jacobian_solver;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -62,13 +62,11 @@ class PDController
PDController();
~PDController();


void init(const std::string & params, std::shared_ptr<rclcpp_lifecycle::LifecycleNode> handle);

double operator()(const double & error, const rclcpp::Duration & period);

private:

std::shared_ptr<rclcpp_lifecycle::LifecycleNode> m_handle;
std::string m_params; ///< namespace for parameter access

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -92,9 +92,9 @@ class SelectivelyDampedLeastSquaresSolver : public IKSolver
* \return True, if everything went well
*/

bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits,
const KDL::JntArray & lower_pos_limits) override;
bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh, const KDL::Chain & chain,
const KDL::JntArray & upper_pos_limits,
const KDL::JntArray & lower_pos_limits) override;

private:
/**
Expand Down
5 changes: 2 additions & 3 deletions cartesian_controller_base/src/IKSolver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -102,10 +102,9 @@ void IKSolver::synchronizeJointPositions(
}
}


bool IKSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> /*nh*/,
const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits,
const KDL::JntArray & lower_pos_limits)
const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits,
const KDL::JntArray & lower_pos_limits)
{
// Initialize
m_chain = chain;
Expand Down
4 changes: 2 additions & 2 deletions cartesian_controller_base/src/JacobianTransposeSolver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -103,8 +103,8 @@ trajectory_msgs::msg::JointTrajectoryPoint JacobianTransposeSolver::getJointCont
}

bool JacobianTransposeSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits,
const KDL::JntArray & lower_pos_limits)
const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits,
const KDL::JntArray & lower_pos_limits)
{
IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -136,11 +136,10 @@ trajectory_msgs::msg::JointTrajectoryPoint SelectivelyDampedLeastSquaresSolver::
return control_cmd;
}


bool SelectivelyDampedLeastSquaresSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
const KDL::Chain & chain,
const KDL::JntArray & upper_pos_limits,
const KDL::JntArray & lower_pos_limits)
const KDL::Chain & chain,
const KDL::JntArray & upper_pos_limits,
const KDL::JntArray & lower_pos_limits)
{
IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);

Expand Down
1 change: 0 additions & 1 deletion cartesian_controller_base/src/SpatialPDController.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,6 @@ ctrl::Vector6D SpatialPDController::operator()(const ctrl::Vector6D & error,
return m_cmd;
}


bool SpatialPDController::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> handle)
{
// Create pd controllers for each Cartesian dimension
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,6 @@ CartesianControllerBase::state_interface_configuration() const
return conf;
}


rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
CartesianControllerBase::on_init()
{
Expand Down
5 changes: 0 additions & 5 deletions cartesian_controller_handles/src/motion_control_handle.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -123,20 +123,15 @@ controller_interface::InterfaceConfiguration MotionControlHandle::state_interfac
return conf;
}


rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
MotionControlHandle::on_init()
{


auto_declare<std::string>("robot_description", "");
auto_declare<std::string>("robot_base_link", "");
auto_declare<std::string>("end_effector_link", "");
auto_declare<std::vector<std::string> >("joints", std::vector<std::string>());


return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;

}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -52,8 +52,6 @@
#include "rclcpp/macros.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"



namespace cartesian_controller_simulation
{
// Two custom hardware interfaces for torque-actuated robots:
Expand Down Expand Up @@ -87,12 +85,9 @@ class Simulator : public hardware_interface::SystemInterface
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces) override;


return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;



private:
// Command buffers for the controllers
std::vector<double> m_position_commands;
Expand Down
9 changes: 0 additions & 9 deletions cartesian_controller_simulation/src/system_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,6 @@ Simulator::CallbackReturn Simulator::on_init(const hardware_interface::HardwareI
"Joint '%s' needs two possible command interfaces.", joint.name.c_str());

return Simulator::CallbackReturn::ERROR;

}

if (!(joint.command_interfaces[0].name == hardware_interface::HW_IF_POSITION ||
Expand All @@ -109,7 +108,6 @@ Simulator::CallbackReturn Simulator::on_init(const hardware_interface::HardwareI
hardware_interface::HW_IF_VELOCITY);

return Simulator::CallbackReturn::ERROR;

}

if (joint.state_interfaces.size() != 3)
Expand All @@ -118,7 +116,6 @@ Simulator::CallbackReturn Simulator::on_init(const hardware_interface::HardwareI
joint.name.c_str());

return Simulator::CallbackReturn::ERROR;

}

if (!(joint.state_interfaces[0].name == hardware_interface::HW_IF_POSITION ||
Expand All @@ -131,12 +128,10 @@ Simulator::CallbackReturn Simulator::on_init(const hardware_interface::HardwareI
hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_EFFORT);

return Simulator::CallbackReturn::ERROR;

}
}

return Simulator::CallbackReturn::SUCCESS;

}

std::vector<hardware_interface::StateInterface> Simulator::export_state_interfaces()
Expand Down Expand Up @@ -182,9 +177,6 @@ Simulator::return_type Simulator::prepare_command_mode_switch(
return return_type::OK;
}




Simulator::return_type Simulator::read([[maybe_unused]] const rclcpp::Time & time,
[[maybe_unused]] const rclcpp::Duration & period)

Expand All @@ -208,7 +200,6 @@ Simulator::return_type Simulator::read([[maybe_unused]] const rclcpp::Time & tim
return return_type::OK;
}


Simulator::return_type Simulator::write([[maybe_unused]] const rclcpp::Time & time,
[[maybe_unused]] const rclcpp::Duration & period)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,6 @@ class CartesianForceController : public virtual cartesian_controller_base::Carte
public:
CartesianForceController();


virtual LifecycleNodeInterface::CallbackReturn on_init() override;

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(
Expand All @@ -91,7 +90,6 @@ class CartesianForceController : public virtual cartesian_controller_base::Carte
controller_interface::return_type update(const rclcpp::Time & time,
const rclcpp::Duration & period) override;


using Base = cartesian_controller_base::CartesianControllerBase;

protected:
Expand Down
3 changes: 0 additions & 3 deletions cartesian_force_controller/src/cartesian_force_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,6 @@ CartesianForceController::CartesianForceController()
{
}


rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
CartesianForceController::on_init()
{
Expand All @@ -67,7 +66,6 @@ CartesianForceController::on_init()
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}


rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
CartesianForceController::on_configure(const rclcpp_lifecycle::State & previous_state)
{
Expand Down Expand Up @@ -120,7 +118,6 @@ CartesianForceController::on_deactivate(const rclcpp_lifecycle::State & previous
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}


controller_interface::return_type CartesianForceController::update(const rclcpp::Time & time,
const rclcpp::Duration & period)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,6 @@ class CartesianMotionController : public virtual cartesian_controller_base::Cart
controller_interface::return_type update(const rclcpp::Time & time,
const rclcpp::Duration & period) override;


using Base = cartesian_controller_base::CartesianControllerBase;

protected:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,6 @@ CartesianMotionController::on_init()
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}


rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
CartesianMotionController::on_configure(const rclcpp_lifecycle::State & previous_state)
{
Expand Down Expand Up @@ -100,7 +99,6 @@ CartesianMotionController::on_deactivate(const rclcpp_lifecycle::State & previou
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}


controller_interface::return_type CartesianMotionController::update(const rclcpp::Time & time,
const rclcpp::Duration & period)
{
Expand Down

0 comments on commit 9a2c5e1

Please sign in to comment.