Class KickEngine

Inheritance Relationships

Base Type

  • public bitbots_splines::AbstractEngine< KickGoals, KickPositions >

Class Documentation

class bitbots_dynamic_kick::KickEngine : public bitbots_splines::AbstractEngine<KickGoals, KickPositions>

The KickEngine takes care of choosing an optimal foot to reach a given goal, planning that foots required movement (rotation and positioning) and updating short-term MotorGoals to move (the foot) along that planned path.

It is vital to call the engines tick() method repeatedly because that is where these short-term MotorGoals are returned.

The KickEngine utilizes a Stabilizer to balance the robot during foot movments.

Public Functions

explicit KickEngine(rclcpp::Node::SharedPtr node)
void setGoals(const KickGoals &goals) override

Set new goal which the engine tries to kick at. This will remove the old goal completely and plan new splines.

Parameters

  • header – Definition of frame and time in which the goals were published

  • ball_position – Position of the ball

  • kick_direction – Direction into which to kick the ball

  • kick_speed – Speed with which to kick the ball

  • r_foot_pose – Current pose of right foot in l_sole frame

  • l_foot_pose – Current pose of left foot in r_sole frame

Throws

tf2::TransformException – when goal cannot be converted into needed tf frames

void reset() override

Reset this KickEngine completely, removing the goal, all splines and thereby stopping all output

KickPositions update(double dt) override

Do one iteration of spline-progress-updating. This means that whenever update() is called, new position goals are retrieved from previously calculated splines, stabilized and transformed into JointGoals

Parameters

dt – Passed delta-time between last call to update() and now. Measured in seconds

Returns

New motor goals only if a goal is currently set, position extractions from splines was possible and IK was able to compute valid motor positions

bool isLeftKick()

Is the currently performed kick with the left foot or not

int getPercentDone() const override
geometry_msgs::msg::Pose getTrunkPose()

Get the current position of the trunk relative to the support foot

bitbots_splines::PoseSpline getFlyingSplines() const
bitbots_splines::PoseSpline getTrunkSplines() const
void setParams(KickParams params)
KickPhase getPhase() const

Get the current phase of the engine

Eigen::Vector3d getWindupPoint()
void setRobotState(moveit::core::RobotStatePtr current_state)

Set a pointer to the current state of the robot, updated from joint states

Private Functions

void calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Eigen::Isometry3d &trunk_pose)

Calculate splines for a complete kick whereby is_left_kick_ should already be set correctly

Parameters

  • flying_foot_pose – Current pose of the flying foot relative to the support foot

  • trunk_pose – Current pose of the trunk relative to the support foot

Eigen::Vector3d calcKickWindupPoint()

Calculate the point from which to perform the final kicking movement

bool calcIsLeftFootKicking(const Eigen::Vector3d &ball_position, const Eigen::Quaterniond &kick_direction)

Choose with which foot the kick should be performed

This is done by checking whether the ball is outside of a corridor ranging from base_footprint forward. If it is, the foot on that side will be chosen as the kicking foot. If not, a more fine grained angle based criterion is used. *

Parameters

  • ball_position – Position where the ball is currently located

  • kick_direction – Direction into which the ball should be kicked

Throws

tf2::TransformException – when ball_position and kick_direction cannot be converted into base_footprint frame

Returns

Whether the resulting kick should be performed with the left foot

double calcKickFootYaw()

Calculate the yaw of the kicking foot, so that it is turned in the direction of the kick

std::pair<Eigen::Vector3d, Eigen::Quaterniond> transformGoal(const std::string &support_foot_frame, const Eigen::Isometry3d &trunk_to_base_footprint, const Eigen::Vector3d &ball_position, const Eigen::Quaterniond &kick_direction)

Transform then goal into our support_foots frame

Parameters

  • support_foot_frame – Name of the support foots frame, meaning where to transform to

  • trunk_to_base_footprint – Pose of the base_footprint relative to the trunk

  • ball_position – Position of the ball

  • kick_direction – Direction in which to kick the ball

Throws

tf2::TransformException – when goal cannot be transformed into support_foot_frame

Returns

pair of (transformed_pose, transformed_direction)

Private Members

rclcpp::Node::SharedPtr node_
double time_
Eigen::Vector3d ball_position_
Eigen::Quaterniond kick_direction_
double kick_speed_
bool is_left_kick_
bitbots_splines::PoseSpline flying_foot_spline_
bitbots_splines::PoseSpline trunk_spline_
KickParams params_
PhaseTimings phase_timings_
Eigen::Vector3d windup_point_
moveit::core::RobotStatePtr current_state_