Skip to content

Commit 7dc2a2b

Browse files
juelgjuelg
committed
format: apply cpp formatting
1 parent edcbc2a commit 7dc2a2b

15 files changed

Lines changed: 185 additions & 181 deletions

File tree

‎extensions/rcs_fr3/src/hw/Franka.cpp‎

Lines changed: 22 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -19,9 +19,9 @@
1919

2020
namespace rcs {
2121
namespace hw {
22-
Franka::Franka(const std::string &ip,
22+
Franka::Franka(const std::string& ip,
2323
std::optional<std::shared_ptr<common::Kinematics>> ik,
24-
const std::optional<FrankaConfig> &cfg)
24+
const std::optional<FrankaConfig>& cfg)
2525
: robot(ip), m_ik(ik) {
2626
// set collision behavior and impedance
2727
this->set_default_robot_behavior();
@@ -35,7 +35,7 @@ Franka::Franka(const std::string &ip,
3535
Franka::~Franka() {
3636
try {
3737
this->stop_control_thread();
38-
} catch (const franka::Exception &e) {
38+
} catch (const franka::Exception& e) {
3939
std::cerr << "Exception in ~Franka(): " << e.what() << std::endl;
4040
}
4141
}
@@ -45,7 +45,7 @@ Franka::~Franka() {
4545
* @param cfg The configuration for the robot, it should be a FrankaConfig type
4646
* otherwise the call will fail
4747
*/
48-
bool Franka::set_config(const FrankaConfig &cfg) {
48+
bool Franka::set_config(const FrankaConfig& cfg) {
4949
this->cfg = cfg;
5050
this->cfg.speed_factor = std::min(std::max(cfg.speed_factor, 0.0), 1.0);
5151

@@ -66,16 +66,16 @@ bool Franka::set_config(const FrankaConfig &cfg) {
6666
return true;
6767
}
6868

69-
FrankaConfig *Franka::get_config() {
69+
FrankaConfig* Franka::get_config() {
7070
// copy config to heap
71-
FrankaConfig *cfg = new FrankaConfig();
71+
FrankaConfig* cfg = new FrankaConfig();
7272
*cfg = this->cfg;
7373
return cfg;
7474
}
7575

76-
FrankaState *Franka::get_state() {
76+
FrankaState* Franka::get_state() {
7777
// dummy state until we define a prober state
78-
FrankaState *state = new FrankaState();
78+
FrankaState* state = new FrankaState();
7979
return state;
8080
}
8181

@@ -105,7 +105,7 @@ common::Pose Franka::get_cartesian_position() {
105105
return x;
106106
}
107107

108-
void Franka::set_joint_position(const common::VectorXd &q) {
108+
void Franka::set_joint_position(const common::VectorXd& q) {
109109
if (this->cfg.async_control) {
110110
this->controller_set_joint_position(q);
111111
return;
@@ -134,7 +134,7 @@ void Franka::set_guiding_mode(bool x, bool y, bool z, bool roll, bool pitch,
134134
this->robot.setGuidingMode(activated, elbow);
135135
}
136136

137-
void PInverse(const Eigen::MatrixXd &M, Eigen::MatrixXd &M_inv,
137+
void PInverse(const Eigen::MatrixXd& M, Eigen::MatrixXd& M_inv,
138138
double epsilon = 0.00025) {
139139
Eigen::JacobiSVD<Eigen::MatrixXd> svd(
140140
M, Eigen::ComputeFullU | Eigen::ComputeFullV);
@@ -153,7 +153,7 @@ void PInverse(const Eigen::MatrixXd &M, Eigen::MatrixXd &M_inv,
153153
M_inv = Eigen::MatrixXd(svd.matrixV() * S_inv * svd.matrixU().transpose());
154154
}
155155

156-
void TorqueSafetyGuardFn(std::array<double, 7> &tau_d_array, double min_torque,
156+
void TorqueSafetyGuardFn(std::array<double, 7>& tau_d_array, double min_torque,
157157
double max_torque) {
158158
for (size_t i = 0; i < tau_d_array.size(); i++) {
159159
if (tau_d_array[i] < min_torque) {
@@ -164,7 +164,7 @@ void TorqueSafetyGuardFn(std::array<double, 7> &tau_d_array, double min_torque,
164164
}
165165
}
166166

167-
void Franka::controller_set_joint_position(const common::Vector7d &desired_q) {
167+
void Franka::controller_set_joint_position(const common::Vector7d& desired_q) {
168168
// from deoxys/config/osc-position-controller.yml
169169
double traj_interpolation_time_fraction = 1.0; // in s
170170
// form deoxys/config/charmander.yml
@@ -199,7 +199,7 @@ void Franka::controller_set_joint_position(const common::Vector7d &desired_q) {
199199
}
200200

201201
void Franka::osc_set_cartesian_position(
202-
const common::Pose &desired_pose_EE_in_base_frame) {
202+
const common::Pose& desired_pose_EE_in_base_frame) {
203203
// from deoxys/config/osc-position-controller.yml
204204
double traj_interpolation_time_fraction = 1.0;
205205
// form deoxys/config/charmander.yml
@@ -293,7 +293,7 @@ void Franka::osc() {
293293
joint_min_ << -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973;
294294
avoidance_weights_ << 1., 1., 1., 1., 1., 10., 10.;
295295

296-
this->robot.control([&](const franka::RobotState &robot_state,
296+
this->robot.control([&](const franka::RobotState& robot_state,
297297
franka::Duration period) -> franka::Torques {
298298
std::chrono::high_resolution_clock::time_point t1 =
299299
std::chrono::high_resolution_clock::now();
@@ -484,7 +484,7 @@ void Franka::joint_controller() {
484484
joint_max_ << 2.8978, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973;
485485
joint_min_ << -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973;
486486

487-
this->robot.control([&](const franka::RobotState &robot_state,
487+
this->robot.control([&](const franka::RobotState& robot_state,
488488
franka::Duration period) -> franka::Torques {
489489
std::chrono::high_resolution_clock::time_point t1 =
490490
std::chrono::high_resolution_clock::now();
@@ -570,7 +570,7 @@ void Franka::zero_torque_controller() {
570570
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
571571

572572
this->controller_time = 0.0;
573-
this->robot.control([&](const franka::RobotState &robot_state,
573+
this->robot.control([&](const franka::RobotState& robot_state,
574574
franka::Duration period) -> franka::Torques {
575575
this->interpolator_mutex.lock();
576576
this->curr_state = robot_state;
@@ -613,7 +613,7 @@ void Franka::double_tap_robot_to_continue() {
613613
auto last_time_something_happened = std::chrono::system_clock::now();
614614
auto last_time_something_touched = std::chrono::system_clock::now();
615615
start_force << s.O_F_ext_hat_K[0], s.O_F_ext_hat_K[1], s.O_F_ext_hat_K[2];
616-
this->robot.read([&](const franka::RobotState &robot_state) {
616+
this->robot.read([&](const franka::RobotState& robot_state) {
617617
Eigen::Vector3d force;
618618
force << robot_state.O_F_ext_hat_K[0], robot_state.O_F_ext_hat_K[1],
619619
robot_state.O_F_ext_hat_K[2];
@@ -663,7 +663,7 @@ std::optional<std::shared_ptr<common::Kinematics>> Franka::get_ik() {
663663
return this->m_ik;
664664
}
665665

666-
void Franka::set_cartesian_position(const common::Pose &x) {
666+
void Franka::set_cartesian_position(const common::Pose& x) {
667667
// pose is assumed to be in the robots coordinate frame
668668
if (this->cfg.async_control) {
669669
this->osc_set_cartesian_position(x);
@@ -691,7 +691,7 @@ void Franka::set_cartesian_position(const common::Pose &x) {
691691
}
692692
}
693693

694-
void Franka::set_cartesian_position_ik(const common::Pose &pose) {
694+
void Franka::set_cartesian_position_ik(const common::Pose& pose) {
695695
if (!this->m_ik.has_value()) {
696696
throw std::runtime_error(
697697
"No inverse kinematics was provided. Cannot use IK to set cartesian "
@@ -714,14 +714,14 @@ common::Pose Franka::get_base_pose_in_world_coordinates() {
714714
: common::Pose();
715715
}
716716

717-
void Franka::set_cartesian_position_internal(const common::Pose &pose,
717+
void Franka::set_cartesian_position_internal(const common::Pose& pose,
718718
double max_time,
719719
std::optional<double> elbow,
720720
std::optional<double> max_force) {
721721
// TODO: use speed factor instead of max_time
722722
common::Pose initial_pose = this->get_cartesian_position();
723723

724-
auto force_stop_condition = [&max_force](const franka::RobotState &state,
724+
auto force_stop_condition = [&max_force](const franka::RobotState& state,
725725
const double progress) {
726726
Eigen::Vector3d force;
727727
force << state.O_F_ext_hat_K[0], state.O_F_ext_hat_K[1],
@@ -737,7 +737,7 @@ void Franka::set_cartesian_position_internal(const common::Pose &pose,
737737
this->robot.control(
738738
[&force_stop_condition, &initial_elbow, &elbow, &max_force, &time,
739739
&max_time, &initial_pose, &should_stop,
740-
&pose](const franka::RobotState &state,
740+
&pose](const franka::RobotState& state,
741741
franka::Duration time_step) -> franka::CartesianPose {
742742
time += time_step.toSec();
743743
if (time == 0) {

‎extensions/rcs_fr3/src/hw/Franka.h‎

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -68,31 +68,31 @@ class Franka : public common::Robot {
6868
void zero_torque_controller();
6969

7070
public:
71-
Franka(const std::string &ip,
71+
Franka(const std::string& ip,
7272
std::optional<std::shared_ptr<common::Kinematics>> ik = std::nullopt,
73-
const std::optional<FrankaConfig> &cfg = std::nullopt);
73+
const std::optional<FrankaConfig>& cfg = std::nullopt);
7474
~Franka() override;
7575

76-
bool set_config(const FrankaConfig &cfg);
76+
bool set_config(const FrankaConfig& cfg);
7777

78-
FrankaConfig *get_config() override;
78+
FrankaConfig* get_config() override;
7979

80-
FrankaState *get_state() override;
80+
FrankaState* get_state() override;
8181

8282
void set_default_robot_behavior();
8383

8484
common::Pose get_cartesian_position() override;
8585

86-
void set_joint_position(const common::VectorXd &q) override;
86+
void set_joint_position(const common::VectorXd& q) override;
8787

8888
common::VectorXd get_joint_position() override;
8989

9090
void set_guiding_mode(bool x, bool y, bool z, bool roll, bool pitch, bool yaw,
9191
bool elbow);
9292

93-
void controller_set_joint_position(const common::Vector7d &desired_q);
93+
void controller_set_joint_position(const common::Vector7d& desired_q);
9494
void osc_set_cartesian_position(
95-
const common::Pose &desired_pose_EE_in_base_frame);
95+
const common::Pose& desired_pose_EE_in_base_frame);
9696
void zero_torque_guiding();
9797

9898
void stop_control_thread();
@@ -105,16 +105,16 @@ class Franka : public common::Robot {
105105

106106
void double_tap_robot_to_continue();
107107

108-
void set_cartesian_position(const common::Pose &pose) override;
108+
void set_cartesian_position(const common::Pose& pose) override;
109109

110110
std::optional<std::shared_ptr<common::Kinematics>> get_ik() override;
111111

112-
void set_cartesian_position_internal(const common::Pose &pose,
112+
void set_cartesian_position_internal(const common::Pose& pose,
113113
double max_time,
114114
std::optional<double> elbow,
115115
std::optional<double> max_force = 5);
116116

117-
void set_cartesian_position_ik(const common::Pose &x);
117+
void set_cartesian_position_ik(const common::Pose& x);
118118

119119
common::Pose get_base_pose_in_world_coordinates() override;
120120

‎extensions/rcs_fr3/src/hw/FrankaHand.cpp‎

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
namespace rcs {
1313
namespace hw {
1414

15-
FrankaHand::FrankaHand(const std::string &ip, const FHConfig &cfg)
15+
FrankaHand::FrankaHand(const std::string& ip, const FHConfig& cfg)
1616
: gripper(ip), cfg{} {
1717
this->cfg = cfg;
1818
this->m_reset();
@@ -21,12 +21,12 @@ FrankaHand::FrankaHand(const std::string &ip, const FHConfig &cfg)
2121
FrankaHand::~FrankaHand() {
2222
try {
2323
this->m_stop();
24-
} catch (const franka::Exception &e) {
24+
} catch (const franka::Exception& e) {
2525
std::cerr << "Exception in ~FrankaHand(): " << e.what() << std::endl;
2626
}
2727
}
2828

29-
bool FrankaHand::set_config(const FHConfig &cfg) {
29+
bool FrankaHand::set_config(const FHConfig& cfg) {
3030
franka::GripperState gripper_state = this->gripper.readOnce();
3131
if (gripper_state.max_width < cfg.grasping_width) {
3232
return false;
@@ -35,19 +35,19 @@ bool FrankaHand::set_config(const FHConfig &cfg) {
3535
return true;
3636
}
3737

38-
FHConfig *FrankaHand::get_config() {
38+
FHConfig* FrankaHand::get_config() {
3939
// copy config to heap
40-
FHConfig *cfg = new FHConfig();
40+
FHConfig* cfg = new FHConfig();
4141
*cfg = this->cfg;
4242
return cfg;
4343
}
4444

45-
FHState *FrankaHand::get_state() {
45+
FHState* FrankaHand::get_state() {
4646
franka::GripperState gripper_state = this->gripper.readOnce();
4747
if (std::abs(gripper_state.max_width - this->cfg.grasping_width) > 0.01) {
4848
this->max_width = gripper_state.max_width - 0.001;
4949
}
50-
FHState *state = new FHState();
50+
FHState* state = new FHState();
5151
state->width = gripper_state.width / gripper_state.max_width;
5252
state->is_grasped = gripper_state.is_grasped;
5353
state->temperature = gripper_state.temperature;
@@ -81,7 +81,7 @@ double FrankaHand::get_normalized_width() {
8181
void FrankaHand::m_stop() {
8282
try {
8383
this->gripper.stop();
84-
} catch (const franka::Exception &e) {
84+
} catch (const franka::Exception& e) {
8585
std::cerr << "FrankaHand::m_stop: " << e.what() << std::endl;
8686
}
8787
this->m_wait();
@@ -137,7 +137,7 @@ void FrankaHand::grasp() {
137137
this->is_moving = true;
138138
try {
139139
this->gripper.grasp(0, this->cfg.speed, this->cfg.force, 1, 1);
140-
} catch (const franka::CommandException &e) {
140+
} catch (const franka::CommandException& e) {
141141
std::cerr << "franka hand command exception ignored grasp" << std::endl;
142142
}
143143
this->is_moving = false;
@@ -163,7 +163,7 @@ void FrankaHand::open() {
163163
this->gripper.move(this->max_width, this->cfg.speed);
164164
// this->gripper.grasp(this->max_width, this->cfg.speed, this->cfg.force,
165165
// 1, 1);
166-
} catch (const franka::CommandException &e) {
166+
} catch (const franka::CommandException& e) {
167167
std::cerr << "franka hand command exception ignored open" << std::endl;
168168
}
169169
this->is_moving = false;

‎extensions/rcs_fr3/src/hw/FrankaHand.h‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -54,14 +54,14 @@ class FrankaHand : public common::Gripper {
5454
void m_wait();
5555

5656
public:
57-
FrankaHand(const std::string &ip, const FHConfig &cfg);
57+
FrankaHand(const std::string& ip, const FHConfig& cfg);
5858
~FrankaHand() override;
5959

60-
bool set_config(const FHConfig &cfg);
60+
bool set_config(const FHConfig& cfg);
6161

62-
FHConfig *get_config() override;
62+
FHConfig* get_config() override;
6363

64-
FHState *get_state() override;
64+
FHState* get_state() override;
6565

6666
void reset() override;
6767

‎extensions/rcs_fr3/src/hw/main.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ int main() {
3333
robot.set_cartesian_position_internal(rs, 5.0, std::nullopt);
3434

3535
// robot.automatic_error_recovery();
36-
} catch (const franka::Exception &e) {
36+
} catch (const franka::Exception& e) {
3737
cout << e.what() << endl;
3838
return -1;
3939
}

‎extensions/rcs_fr3/src/pybind/rcs.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -104,7 +104,7 @@ PYBIND11_MODULE(_core, m) {
104104
(py::object)py::module_::import("rcs").attr("common").attr("Robot");
105105
py::class_<rcs::hw::Franka, std::shared_ptr<rcs::hw::Franka>>(hw, "Franka",
106106
robot)
107-
.def(py::init<const std::string &,
107+
.def(py::init<const std::string&,
108108
std::optional<std::shared_ptr<rcs::common::Kinematics>>>(),
109109
py::arg("ip"), py::arg("ik") = std::nullopt)
110110
.def("set_config", &rcs::hw::Franka::set_config, py::arg("cfg"))
@@ -138,7 +138,7 @@ PYBIND11_MODULE(_core, m) {
138138
(py::object)py::module_::import("rcs").attr("common").attr("Gripper");
139139
py::class_<rcs::hw::FrankaHand, std::shared_ptr<rcs::hw::FrankaHand>>(
140140
hw, "FrankaHand", gripper)
141-
.def(py::init<const std::string &, const rcs::hw::FHConfig &>(),
141+
.def(py::init<const std::string&, const rcs::hw::FHConfig&>(),
142142
py::arg("ip"), py::arg("cfg"))
143143
.def("get_config", &rcs::hw::FrankaHand::get_config)
144144
.def("get_state", &rcs::hw::FrankaHand::get_state)

0 commit comments

Comments
 (0)