Skip to content

Commit 5eb2ac0

Browse files
TimSchneider42TimSchneider42
committed
Renamed toStd to toStdD for consistency
1 parent 4a6214f commit 5eb2ac0

10 files changed

+51
-46
lines changed

‎include/franky/motion/position_waypoint_motion.hpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -55,9 +55,9 @@ class PositionWaypointMotion : public WaypointMotion<ControlSignalType, Position
5555
auto relative_dynamics_factor =
5656
waypoint.relative_dynamics_factor * relative_dynamics_factor_ * this->robot()->relative_dynamics_factor();
5757

58-
input_parameter.max_velocity = toStd<7>(relative_dynamics_factor.velocity() * vel_lim);
59-
input_parameter.max_acceleration = toStd<7>(relative_dynamics_factor.acceleration() * acc_lim);
60-
input_parameter.max_jerk = toStd<7>(relative_dynamics_factor.jerk() * jerk_lim);
58+
input_parameter.max_velocity = toStdD<7>(relative_dynamics_factor.velocity() * vel_lim);
59+
input_parameter.max_acceleration = toStdD<7>(relative_dynamics_factor.acceleration() * acc_lim);
60+
input_parameter.max_jerk = toStdD<7>(relative_dynamics_factor.jerk() * jerk_lim);
6161

6262
if (relative_dynamics_factor.max_dynamics()) {
6363
input_parameter.synchronization = ruckig::Synchronization::TimeIfNecessary;

‎include/franky/motion/velocity_waypoint_motion.hpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -48,9 +48,9 @@ class VelocityWaypointMotion : public WaypointMotion<ControlSignalType, Velocity
4848
auto relative_dynamics_factor =
4949
waypoint.relative_dynamics_factor * relative_dynamics_factor_ * this->robot()->relative_dynamics_factor();
5050

51-
input_parameter.max_velocity = toStd<7>(relative_dynamics_factor.acceleration() * acc_lim);
52-
input_parameter.max_acceleration = toStd<7>(relative_dynamics_factor.jerk() * jerk_lim);
53-
input_parameter.max_jerk = toStd<7>(Vector7d::Constant(std::numeric_limits<double>::infinity()));
51+
input_parameter.max_velocity = toStdD<7>(relative_dynamics_factor.acceleration() * acc_lim);
52+
input_parameter.max_acceleration = toStdD<7>(relative_dynamics_factor.jerk() * jerk_lim);
53+
input_parameter.max_jerk = toStdD<7>(Vector7d::Constant(std::numeric_limits<double>::infinity()));
5454

5555
if (relative_dynamics_factor.max_dynamics()) {
5656
input_parameter.synchronization = ruckig::Synchronization::TimeIfNecessary;

‎include/franky/util.hpp‎

Lines changed: 13 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -6,25 +6,30 @@
66

77
namespace franky {
88

9-
template<size_t dims>
10-
std::array<double, dims> toStd(const Eigen::Matrix<double, dims, 1> &vector) {
11-
std::array<double, dims> result;
12-
Eigen::Matrix<double, dims, 1>::Map(result.data()) = vector;
9+
template<typename T, size_t dims>
10+
std::array<T, dims> toStd(const Eigen::Matrix<T, dims, 1> &vector) {
11+
std::array<T, dims> result;
12+
Eigen::Matrix<T, dims, 1>::Map(result.data()) = vector;
1313
return result;
1414
}
1515

16-
template<size_t dims, typename T>
16+
template<size_t dims>
17+
std::array<double, dims> toStdD(const Eigen::Matrix<double, dims, 1> &vector) {
18+
return toStd<double, dims>(vector);
19+
}
20+
21+
template<typename T, size_t dims>
1722
Eigen::Matrix<T, dims, 1> toEigen(const std::array<T, dims> &vector) {
1823
return Eigen::Matrix<T, dims, 1>::Map(vector.data());
1924
}
2025

2126
template<size_t dims>
2227
Eigen::Matrix<double, dims, 1> toEigenD(const std::array<double, dims> &vector) {
23-
return Eigen::Matrix<double, dims, 1>::Map(vector.data());
28+
return toEigen<double, dims>(vector);
2429
}
2530

2631
template<size_t rows, size_t cols>
27-
std::array<double, rows * cols> toStdMatD(const Eigen::Matrix<double, rows, cols, Eigen::ColMajor> &matrix) {
32+
std::array<double, rows * cols> toStdDMatD(const Eigen::Matrix<double, rows, cols, Eigen::ColMajor> &matrix) {
2833
std::array<double, rows * cols> result;
2934
Eigen::Map<Eigen::Matrix<double, rows, cols, Eigen::ColMajor>>(result.data()) = matrix;
3035
return result;
@@ -46,7 +51,7 @@ template<size_t dims>
4651
std::array<double, dims> ensureStd(const Array<dims> &input) {
4752
if (std::holds_alternative<std::array<double, dims >>(input))
4853
return std::get<std::array<double, dims >>(input);
49-
return toStd<dims>(std::get<Eigen::Vector<double, dims >>(input));
54+
return toStdD<dims>(std::get<Eigen::Vector<double, dims >>(input));
5055
}
5156

5257
template<size_t dims>

‎src/model.cpp‎

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ Affine Model::pose(franka::Frame frame,
2525
const Affine &F_T_EE,
2626
const Affine &EE_T_K) const {
2727
return Affine{Eigen::Map<const Eigen::Matrix4d>(
28-
model_.pose(frame, toStd<7>(q), toStdMatD<4, 4>(F_T_EE.matrix()), toStdMatD<4, 4>(EE_T_K.matrix())).data())};
28+
model_.pose(frame, toStdD<7>(q), toStdDMatD<4, 4>(F_T_EE.matrix()), toStdDMatD<4, 4>(EE_T_K.matrix())).data())};
2929
}
3030

3131
Jacobian Model::bodyJacobian(franka::Frame frame, const franka::RobotState &state) const {
@@ -37,7 +37,7 @@ Jacobian Model::bodyJacobian(franka::Frame frame,
3737
const Affine &F_T_EE,
3838
const Affine &EE_T_K) const {
3939
return toEigenMatD<6, 7>(
40-
model_.bodyJacobian(frame, toStd<7>(q), toStdMatD<4, 4>(F_T_EE.matrix()), toStdMatD<4, 4>(EE_T_K.matrix())));
40+
model_.bodyJacobian(frame, toStdD<7>(q), toStdDMatD<4, 4>(F_T_EE.matrix()), toStdDMatD<4, 4>(EE_T_K.matrix())));
4141
}
4242

4343
Jacobian Model::zeroJacobian(franka::Frame frame, const franka::RobotState &state) const {
@@ -49,7 +49,7 @@ Jacobian Model::zeroJacobian(franka::Frame frame,
4949
const Affine &F_T_EE,
5050
const Affine &EE_T_K) const {
5151
return toEigenMatD<6, 7>(
52-
model_.zeroJacobian(frame, toStd<7>(q), toStdMatD<4, 4>(F_T_EE.matrix()), toStdMatD<4, 4>(EE_T_K.matrix())));
52+
model_.zeroJacobian(frame, toStdD<7>(q), toStdDMatD<4, 4>(F_T_EE.matrix()), toStdDMatD<4, 4>(EE_T_K.matrix())));
5353
}
5454

5555
Eigen::Matrix<double, 7, 7> Model::mass(const franka::RobotState &state) const {
@@ -60,7 +60,7 @@ Eigen::Matrix<double, 7, 7> Model::mass(const Vector7d &q,
6060
const Eigen::Matrix3d &I_total,
6161
double m_total,
6262
const Eigen::Vector3d &F_x_Ctotal) const {
63-
return toEigenMatD<7, 7>(model_.mass(toStd<7>(q), toStdMatD<3, 3>(I_total), m_total, toStd<3>(F_x_Ctotal)));
63+
return toEigenMatD<7, 7>(model_.mass(toStdD<7>(q), toStdDMatD<3, 3>(I_total), m_total, toStdD<3>(F_x_Ctotal)));
6464
}
6565

6666
Vector7d Model::coriolis(const franka::RobotState &state) const {
@@ -72,16 +72,16 @@ Vector7d Model::coriolis(const Vector7d &q,
7272
const Eigen::Matrix3d &I_total,
7373
double m_total,
7474
const Eigen::Vector3d &F_x_Ctotal) const {
75-
return toEigenD<7>(model_.coriolis(toStd<7>(q),
76-
toStd<7>(dq),
77-
toStdMatD<3, 3>(I_total),
75+
return toEigenD<7>(model_.coriolis(toStdD<7>(q),
76+
toStdD<7>(dq),
77+
toStdDMatD<3, 3>(I_total),
7878
m_total,
79-
toStd<3>(F_x_Ctotal)));
79+
toStdD<3>(F_x_Ctotal)));
8080
}
8181

8282
Vector7d Model::gravity(const franka::RobotState &state,
8383
const Eigen::Vector3d &gravity_earth) const {
84-
return toEigenD<7>(model_.gravity(state, toStd<3>(gravity_earth)));
84+
return toEigenD<7>(model_.gravity(state, toStdD<3>(gravity_earth)));
8585
}
8686

8787
Vector7d Model::gravity(const franka::RobotState &state) const {
@@ -92,7 +92,7 @@ Vector7d Model::gravity(const Vector7d &q,
9292
double m_total,
9393
const Eigen::Vector3d &F_x_Ctotal,
9494
const Eigen::Vector3d &gravity_earth) const {
95-
return toEigenD<7>(model_.gravity(toStd<7>(q), m_total, toStd<3>(F_x_Ctotal), toStd<3>(gravity_earth)));
95+
return toEigenD<7>(model_.gravity(toStdD<7>(q), m_total, toStdD<3>(F_x_Ctotal), toStdD<3>(gravity_earth)));
9696
}
9797

9898
} // namespace franky

‎src/motion/cartesian_velocity_waypoint_motion.cpp‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -35,9 +35,9 @@ void CartesianVelocityWaypointMotion::initWaypointMotion(
3535
auto initial_acceleration = Vector6d::Map(robot_state.O_ddP_EE_c.data());
3636
Vector7d initial_acceleration_with_elbow = (Vector7d() << initial_acceleration, robot_state.ddelbow_c[0]).finished();
3737

38-
input_parameter.current_position = toStd<7>(current_velocity.vector_repr());
39-
input_parameter.current_velocity = toStd<7>(initial_acceleration_with_elbow);
40-
input_parameter.current_acceleration = toStd<7>(Vector7d::Zero());
38+
input_parameter.current_position = toStdD<7>(current_velocity.vector_repr());
39+
input_parameter.current_velocity = toStdD<7>(initial_acceleration_with_elbow);
40+
input_parameter.current_acceleration = toStdD<7>(Vector7d::Zero());
4141

4242
if (previous_command.has_value())
4343
last_elbow_pos_ = previous_command.value().elbow[0];
@@ -85,8 +85,8 @@ void CartesianVelocityWaypointMotion::setNewWaypoint(
8585
auto new_target_transformed = new_waypoint.target.changeEndEffectorFrame(ee_frame_.inverse().translation());
8686
// This is a bit of an oversimplification, as the angular velocities don't work like linear velocities (but we pretend
8787
// they do here). However, it is probably good enough here.
88-
input_parameter.target_position = toStd<7>(new_target_transformed.vector_repr());
89-
input_parameter.target_velocity = toStd<7>(Vector7d::Zero());
88+
input_parameter.target_position = toStdD<7>(new_target_transformed.vector_repr());
89+
input_parameter.target_velocity = toStdD<7>(Vector7d::Zero());
9090
input_parameter.enabled = {true, true, true, true, true, true, new_target_transformed.elbow_velocity().has_value()};
9191
}
9292

‎src/motion/cartesian_waypoint_motion.cpp‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -28,9 +28,9 @@ void CartesianWaypointMotion::initWaypointMotion(
2828

2929
target_state_ = robot_pose;
3030

31-
input_parameter.current_position = toStd<7>(robot_pose.vector_repr());
32-
input_parameter.current_velocity = toStd<7>(initial_velocity.vector_repr());
33-
input_parameter.current_acceleration = toStd<7>(initial_acceleration_with_elbow);
31+
input_parameter.current_position = toStdD<7>(robot_pose.vector_repr());
32+
input_parameter.current_velocity = toStdD<7>(initial_velocity.vector_repr());
33+
input_parameter.current_acceleration = toStdD<7>(initial_acceleration_with_elbow);
3434
}
3535

3636
franka::CartesianPose CartesianWaypointMotion::getControlSignal(
@@ -77,9 +77,9 @@ void CartesianWaypointMotion::setNewWaypoint(
7777
Vector7d current_velocity_ref_frame =
7878
(Vector7d() << linear_vel_ref_frame, angular_vel_ref_frame, elbow_velocity).finished();
7979
Vector7d current_acc_ref_frame = (Vector7d() << linear_acc_ref_frame, angular_acc_ref_frame, elbow_acc).finished();
80-
input_parameter.current_position = toStd<7>(zero_pose.vector_repr());
81-
input_parameter.current_velocity = toStd<7>(current_velocity_ref_frame);
82-
input_parameter.current_acceleration = toStd<7>(current_acc_ref_frame);
80+
input_parameter.current_position = toStdD<7>(zero_pose.vector_repr());
81+
input_parameter.current_velocity = toStdD<7>(current_velocity_ref_frame);
82+
input_parameter.current_acceleration = toStdD<7>(current_acc_ref_frame);
8383

8484
auto waypoint_pose = new_waypoint.target.pose();
8585

@@ -107,10 +107,10 @@ void CartesianWaypointMotion::setNewWaypoint(
107107
auto new_target_transformed = new_target.changeEndEffectorFrame(ee_frame_.inverse());
108108
auto new_target_ref_frame = ref_frame_.inverse() * new_target_transformed;
109109

110-
input_parameter.target_position = toStd<7>(new_target_ref_frame.pose().vector_repr());
110+
input_parameter.target_position = toStdD<7>(new_target_ref_frame.pose().vector_repr());
111111
// This is a bit of an oversimplification, as the angular velocities don't work like linear velocities (but we pretend
112112
// they do here). However, it is probably good enough here.
113-
input_parameter.target_velocity = toStd<7>(new_target_ref_frame.velocity().vector_repr());
113+
input_parameter.target_velocity = toStdD<7>(new_target_ref_frame.velocity().vector_repr());
114114
input_parameter.enabled = {true, true, true, true, true, true, waypoint_pose.elbow_state().has_value()};
115115

116116
target_state_ = new_target;

‎src/motion/joint_velocity_waypoint_motion.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ void JointVelocityWaypointMotion::initWaypointMotion(const franka::RobotState &r
2525
else
2626
input_parameter.current_position = robot_state.dq_d;
2727
input_parameter.current_velocity = robot_state.ddq_d;
28-
input_parameter.current_acceleration = toStd<7>(Vector7d::Zero());
28+
input_parameter.current_acceleration = toStdD<7>(Vector7d::Zero());
2929
}
3030

3131
franka::JointVelocities JointVelocityWaypointMotion::getControlSignal(
@@ -39,9 +39,9 @@ void JointVelocityWaypointMotion::setNewWaypoint(const franka::RobotState &robot
3939
const VelocityWaypoint<Vector7d> &new_waypoint,
4040
ruckig::InputParameter<7> &input_parameter) {
4141
auto new_target = new_waypoint.target;
42-
input_parameter.target_position = toStd<7>(new_target);
43-
input_parameter.target_velocity = toStd<7>(Vector7d::Zero());
44-
input_parameter.target_acceleration = toStd<7>(Vector7d::Zero());
42+
input_parameter.target_position = toStdD<7>(new_target);
43+
input_parameter.target_velocity = toStdD<7>(Vector7d::Zero());
44+
input_parameter.target_acceleration = toStdD<7>(Vector7d::Zero());
4545
}
4646

4747
std::tuple<Vector7d, Vector7d, Vector7d> JointVelocityWaypointMotion::getAbsoluteInputLimits() const {

‎src/motion/joint_waypoint_motion.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -42,9 +42,9 @@ void JointWaypointMotion::setNewWaypoint(
4242
auto position = new_target.position();
4343
if (new_waypoint.reference_type == ReferenceType::kRelative)
4444
position += toEigen(input_parameter.current_position);
45-
input_parameter.target_position = toStd<7>(position);
46-
input_parameter.target_velocity = toStd<7>(new_target.velocity());
47-
input_parameter.target_acceleration = toStd<7>(Vector7d::Zero());
45+
input_parameter.target_position = toStdD<7>(position);
46+
input_parameter.target_velocity = toStdD<7>(new_target.velocity());
47+
input_parameter.target_acceleration = toStdD<7>(Vector7d::Zero());
4848
}
4949

5050
std::tuple<Vector7d, Vector7d, Vector7d> JointWaypointMotion::getAbsoluteInputLimits() const {

‎src/robot_pose.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ Vector7d RobotPose::vector_repr() const {
4040
}
4141

4242
franka::CartesianPose RobotPose::as_franka_pose(FlipDirection default_flip_direction) const {
43-
std::array<double, 16> array = toStd<16>(Eigen::Map<const Eigen::Vector<double, 16>>(end_effector_pose_.data()));
43+
std::array<double, 16> array = toStdD<16>(Eigen::Map<const Eigen::Vector<double, 16>>(end_effector_pose_.data()));
4444
if (elbow_state_.has_value()) {
4545
return {array, elbow_state_.value().to_array(default_flip_direction)};
4646
}

‎src/robot_velocity.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ Vector7d RobotVelocity::vector_repr() const {
4040

4141
franka::CartesianVelocities RobotVelocity::as_franka_velocity(
4242
const std::optional<ElbowState> &elbow_state, FlipDirection default_elbow_flip_direction) const {
43-
std::array<double, 6> array = toStd<6>(vector_repr().head<6>());
43+
std::array<double, 6> array = toStdD<6>(vector_repr().head<6>());
4444
if (elbow_state.has_value())
4545
return {array, elbow_state->to_array(default_elbow_flip_direction)};
4646
return {array};

0 commit comments

Comments
 (0)