Skip to content

Commit 5d7594a

Browse files
fctalbotfctalbot
committed
Various whitespace changes to fit lines within 120 chars
1 parent 83c7c11 commit 5d7594a

File tree

8 files changed

+123
-125
lines changed

8 files changed

+123
-125
lines changed

‎include/syropod_highlevel_controller/pose.h‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -54,8 +54,11 @@ class Pose
5454
/// @param[in] transform The geometry_msgs/Transform input to be used to construct the pose
5555
inline Pose(const geometry_msgs::Transform& transform)
5656
{
57-
position_ = Eigen::Vector3d(transform.translation.x, transform.translation.y, transform.translation.z);
58-
rotation_ = Eigen::Quaterniond(transform.rotation.w,transform.rotation.x,transform.rotation.y,transform.rotation.z);
57+
position_ = Eigen::Vector3d(transform.translation.x, transform.translation.y, transform.translation.z);
58+
rotation_ = Eigen::Quaterniond(transform.rotation.w,
59+
transform.rotation.x,
60+
transform.rotation.y,
61+
transform.rotation.z);
5962
};
6063

6164
/// Returns a conversion of this pose object into a geometry_msgs::Pose.

‎include/syropod_highlevel_controller/walk_controller.h‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -235,8 +235,7 @@ class WalkController : public std::enable_shared_from_this<WalkController>
235235
WalkState walk_state_ = STOPPED; ///< The current walk cycle state
236236
PosingState pose_state_ = POSING_COMPLETE; ///< The current state of auto posing
237237

238-
// Step cycle timing object
239-
StepCycle step_; ///< TBD
238+
StepCycle step_; ///< Step cycle timing object
240239

241240
// Workspace generation variables
242241
LimitMap walkspace_; ///< A map of interpolated radii for given bearings in degrees at default stance

‎src/admittance_controller.cpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,8 @@ void AdmittanceController::updateAdmittance(void)
2727
{
2828
std::shared_ptr<Leg> leg = leg_it->second;
2929
Eigen::Vector3d admittance_delta = Eigen::Vector3d::Zero();
30-
Eigen::Vector3d tip_force = params_.use_joint_effort.data ? leg->getTipForceCalculated():leg->getTipForceMeasured();
30+
bool use_calculated_tip_force = params_.use_joint_effort.data;
31+
Eigen::Vector3d tip_force = use_calculated_tip_force ? leg->getTipForceCalculated() : leg->getTipForceMeasured();
3132
tip_force *= params_.force_gain.current_value;
3233
for (int i = 0; i < 3; ++i)
3334
{
@@ -132,4 +133,4 @@ void AdmittanceController::updateStiffness(std::shared_ptr<WalkController> walke
132133
}
133134
}
134135

135-
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
136+
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

‎src/debug_visualiser.cpp‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -385,9 +385,8 @@ void DebugVisualiser::generateWalkspace(std::shared_ptr<Leg> leg, const LimitMap
385385
walkspace_marker.color.b = 1;
386386
walkspace_marker.color.a = 1;
387387
Pose pose(Eigen::Vector3d::Zero(), Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(),
388-
leg_stepper->getWalkPlaneNormal()));
388+
leg_stepper->getWalkPlaneNormal()));
389389
walkspace_marker.pose = pose.toPoseMessage();
390-
391390
geometry_msgs::Point origin_point;
392391
Eigen::Vector3d walkspace_origin = leg_stepper->getDefaultTipPose().position_;
393392
walkspace_origin = pose.inverseTransformVector(walkspace_origin);

‎src/model.cpp‎

Lines changed: 34 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -122,7 +122,7 @@ void Model::generateWorkspaces(void)
122122
{
123123
// Create copy of model for searching for kinematic limitations
124124
std::shared_ptr<Model> search_model = std::allocate_shared<Model>(Eigen::aligned_allocator<Model>(),
125-
shared_from_this());
125+
shared_from_this());
126126
search_model->generate(shared_from_this());
127127
search_model->initLegs(true);
128128

@@ -222,8 +222,8 @@ Leg::Leg(std::shared_ptr<Leg> leg, std::shared_ptr<Model> model)
222222

223223
void Leg::generate(std::shared_ptr<Leg> leg)
224224
{
225-
std::shared_ptr<Joint> null_joint = std::allocate_shared<Joint>(Eigen::aligned_allocator<Joint>()); // Null joint
226-
// acts as origin
225+
// Null joint acts as origin
226+
std::shared_ptr<Joint> null_joint = std::allocate_shared<Joint>(Eigen::aligned_allocator<Joint>());
227227
std::shared_ptr<Link> base_link =
228228
std::allocate_shared<Link>(Eigen::aligned_allocator<Link>(), shared_from_this(), null_joint, 0, params_);
229229
link_container_.insert(LinkContainer::value_type(0, base_link));
@@ -344,8 +344,8 @@ Workspace Leg::generateWorkspace(void)
344344

345345
// Calculate Identity tip pose
346346
Pose current_pose = model_->getCurrentPose();
347-
Eigen::Vector3d identity_tip_position = current_pose.inverseTransformVector(leg_stepper_->
348-
getIdentityTipPose().position_);
347+
Eigen::Vector3d identity_tip_position =
348+
current_pose.inverseTransformVector(leg_stepper_-> getIdentityTipPose().position_);
349349

350350
// Set zero workspace if unable to reach idenity_tip_pose
351351
if ((identity_tip_position - current_tip_pose_.position_).norm() > IK_TOLERANCE)
@@ -376,8 +376,8 @@ Workspace Leg::generateWorkspace(void)
376376
while(true)
377377
{
378378
Pose current_pose = model_->getCurrentPose();
379-
Eigen::Vector3d identity_tip_position = current_pose.inverseTransformVector(leg_stepper_->
380-
getIdentityTipPose().position_);
379+
Eigen::Vector3d identity_tip_position =
380+
current_pose.inverseTransformVector(leg_stepper_->getIdentityTipPose().position_);
381381
identity_tip_position[2] += search_height;
382382

383383
// Set origin and target for linear interpolation
@@ -390,8 +390,8 @@ Workspace Leg::generateWorkspace(void)
390390
{
391391
number_iterations = roundToInt(MAX_WORKSPACE_RADIUS / MAX_POSITION_DELTA);
392392
origin_tip_position = identity_tip_position;
393-
Eigen::Vector3d search_limit = (found_lower_limit ? MAX_WORKSPACE_RADIUS : -MAX_WORKSPACE_RADIUS) *
394-
Eigen::Vector3d::UnitZ();
393+
Eigen::Vector3d search_limit =
394+
(found_lower_limit ? MAX_WORKSPACE_RADIUS : -MAX_WORKSPACE_RADIUS) * Eigen::Vector3d::UnitZ();
395395
target_tip_position = identity_tip_position + search_limit;
396396
}
397397
// Track to new workplane origin at search height
@@ -575,8 +575,8 @@ Eigen::Vector3d Leg::makeReachable(const Eigen::Vector3d& reference_tip_position
575575
// If test tip position is beyond limit, calculate new position along same workplane bearing within limits
576576
if (distance_to_test > distance_to_limit)
577577
{
578-
Eigen::Vector3d new_tip_position = Eigen::AngleAxisd(raw_bearing, Eigen::Vector3d::UnitZ()) *
579-
(Eigen::Vector3d::UnitX() * distance_to_limit);
578+
Eigen::Vector3d new_tip_position =
579+
Eigen::AngleAxisd(raw_bearing, Eigen::Vector3d::UnitZ()) * (Eigen::Vector3d::UnitX() * distance_to_limit);
580580
new_tip_position[2] = test_tip_position[2];
581581
new_tip_position = pose.transformVector(new_tip_position);
582582
return new_tip_position;
@@ -692,21 +692,18 @@ void Leg::calculateTipForce(void)
692692

693693
// Transpose and invert jacobian
694694
Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(joint_count_, joint_count_);
695-
Eigen::MatrixXd transformation = jacobian * ((jacobian.transpose()*jacobian + sqr(DLS_COEFFICIENT) *
696-
identity).inverse());
695+
Eigen::MatrixXd transformation =
696+
jacobian * ((jacobian.transpose()*jacobian + sqr(DLS_COEFFICIENT) * identity).inverse());
697697

698698
Eigen::VectorXd raw_tip_force_leg_frame = transformation * joint_torques;
699699
Eigen::Quaterniond rotation = (first_joint->getPoseJointFrame()).rotation_;
700700
Eigen::VectorXd raw_tip_force = rotation._transformVector(raw_tip_force_leg_frame.block<3, 1>(0, 0));
701701

702702
// Low pass filter and force gain applied to calculated raw tip force
703703
double s = 0.15; // Smoothing Factor
704-
tip_force_calculated_[0] = s * raw_tip_force[0] * params_.force_gain.current_value + (1 - s) *
705-
tip_force_calculated_[0];
706-
tip_force_calculated_[1] = s * raw_tip_force[1] * params_.force_gain.current_value + (1 - s) *
707-
tip_force_calculated_[1];
708-
tip_force_calculated_[2] = s * raw_tip_force[2] * params_.force_gain.current_value + (1 - s) *
709-
tip_force_calculated_[2];
704+
tip_force_calculated_[0] = s*raw_tip_force[0]*params_.force_gain.current_value + (1 - s)*tip_force_calculated_[0];
705+
tip_force_calculated_[1] = s*raw_tip_force[1]*params_.force_gain.current_value + (1 - s)*tip_force_calculated_[1];
706+
tip_force_calculated_[2] = s*raw_tip_force[2]*params_.force_gain.current_value + (1 - s)*tip_force_calculated_[2];
710707
}
711708

712709
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -727,9 +724,8 @@ void Leg::touchdownDetection(void)
727724

728725
Eigen::VectorXd Leg::solveIK(const Eigen::MatrixXd& delta, const bool& solve_rotation)
729726
{
730-
// Calculate Jacobian from DH matrices along kinematic chain
731-
// ref: robotics.stackexchange.com/questions/2760/computing-inverse-kinematic-with-jacobian-matrices-
732-
// for-6-dof-manipulator
727+
// Calculate Jacobian from DH matrices along kinematic chain. Ref:
728+
// robotics.stackexchange.com/questions/2760/computing-inverse-kinematic-with-jacobian-matrices-for-6-dof-manipulator
733729
std::shared_ptr<Joint> first_joint = joint_container_.begin()->second;
734730
Eigen::Vector3d pe = tip_->getTransformFromJoint(first_joint->id_number_).block<3, 1>(0, 3);
735731
Eigen::Vector3d z0(0, 0, 1);
@@ -774,19 +770,19 @@ Eigen::VectorXd Leg::solveIK(const Eigen::MatrixXd& delta, const bool& solve_rot
774770
double position_range_centre = joint->min_position_ + joint_position_range/2.0;
775771
if (joint_position_range != 0.0)
776772
{
777-
position_limit_cost += sqr(abs(JOINT_LIMIT_COST_WEIGHT * (joint->desired_position_ - position_range_centre) /
778-
joint_position_range));
779-
position_cost_gradient[i] = -sqr(JOINT_LIMIT_COST_WEIGHT) * (joint->desired_position_ - position_range_centre) /
780-
sqr(joint_position_range);
773+
position_limit_cost +=
774+
sqr(abs(JOINT_LIMIT_COST_WEIGHT * (joint->desired_position_ - position_range_centre) / joint_position_range));
775+
position_cost_gradient[i] =
776+
-sqr(JOINT_LIMIT_COST_WEIGHT) * (joint->desired_position_ - position_range_centre) / sqr(joint_position_range);
781777
}
782778

783779
// VELOCITY LIMITS
784780
double joint_velocity_range = 2 * joint->max_angular_speed_;
785781
double velocity_range_centre = 0.0;
786-
velocity_limit_cost += sqr(abs(JOINT_LIMIT_COST_WEIGHT * (joint->desired_velocity_ - velocity_range_centre) /
787-
joint_velocity_range));
788-
velocity_cost_gradient[i] = -sqr(JOINT_LIMIT_COST_WEIGHT) * (joint->desired_velocity_ - velocity_range_centre) /
789-
sqr(joint_velocity_range);
782+
velocity_limit_cost +=
783+
sqr(abs(JOINT_LIMIT_COST_WEIGHT * (joint->desired_velocity_ - velocity_range_centre) / joint_velocity_range));
784+
velocity_cost_gradient[i] =
785+
-sqr(JOINT_LIMIT_COST_WEIGHT) * (joint->desired_velocity_ - velocity_range_centre) / sqr(joint_velocity_range);
790786
}
791787
position_cost_gradient *= (position_limit_cost == 0.0 ? 0.0 : 1.0 / sqrt(position_limit_cost));
792788
velocity_cost_gradient *= (velocity_limit_cost == 0.0 ? 0.0 : 1.0 / sqrt(velocity_limit_cost));
@@ -888,10 +884,10 @@ double Leg::applyIK(const bool& simulation)
888884
applyFK();
889885

890886
// Generate rotation delta vector in reference to the base of the leg
891-
Eigen::Vector3d desired_tip_direction = leg_frame_desired_tip_pose.rotation_.
892-
_transformVector(Eigen::Vector3d::UnitX());
893-
Eigen::Vector3d current_tip_direction = leg_frame_current_tip_pose.rotation_.
894-
_transformVector(Eigen::Vector3d::UnitX());
887+
Eigen::Vector3d desired_tip_direction =
888+
leg_frame_desired_tip_pose.rotation_._transformVector(Eigen::Vector3d::UnitX());
889+
Eigen::Vector3d current_tip_direction =
890+
leg_frame_current_tip_pose.rotation_._transformVector(Eigen::Vector3d::UnitX());
895891
Eigen::Quaterniond difference = Eigen::Quaterniond::FromTwoVectors(current_tip_direction, desired_tip_direction);
896892
Eigen::AngleAxisd axis_rotation(difference.normalized());
897893
Eigen::Vector3d rotation_delta = axis_rotation.axis() * axis_rotation.angle();
@@ -991,8 +987,8 @@ Pose Leg::applyFK(const bool& set_current, const bool& use_actual)
991987

992988
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
993989

994-
Link::Link(std::shared_ptr<Leg> leg, std::shared_ptr<Joint> actuating_joint, const int& id_number,
995-
const Parameters& params)
990+
Link::Link(std::shared_ptr<Leg> leg, std::shared_ptr<Joint> actuating_joint,
991+
const int& id_number, const Parameters& params)
996992
: parent_leg_(leg)
997993
, actuating_joint_(actuating_joint)
998994
, id_number_(id_number)
@@ -1025,8 +1021,8 @@ Link::Link(std::shared_ptr<Link> link)
10251021

10261022
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
10271023

1028-
Joint::Joint(std::shared_ptr<Leg> leg, std::shared_ptr<Link> reference_link, const int& id_number,
1029-
const Parameters& params)
1024+
Joint::Joint(std::shared_ptr<Leg> leg, std::shared_ptr<Link> reference_link,
1025+
const int& id_number, const Parameters& params)
10301026
: parent_leg_(leg)
10311027
, reference_link_(reference_link)
10321028
, id_number_(id_number)

0 commit comments

Comments
 (0)