giskardpy.motion_statechart.tasks.feature_functions

giskardpy.motion_statechart.tasks.feature_functions#

Classes#

`FeatureFunctionGoal`	Base for feature tasks operating on geometric features.
`AlignPerpendicular`	Align two normal vectors to be perpendicular.
`HeightGoal`	Moves the tip_point to be the specified distance away from the reference_point
`DistanceGoal`	Moves the tip_point to be the specified distance away from the reference_point
`ReachPoint`	Drives a controlled point on the tip onto a reference point.
`AngleGoal`	Controls the angle between the tip_vector and the reference_vector to be between

Module Contents#

class giskardpy.motion_statechart.tasks.feature_functions.FeatureFunctionGoal#

Bases: giskardpy.motion_statechart.graph_node.ConvergingTask, abc.ABC

Base for feature tasks operating on geometric features.

Transforms the controlled feature (from tip_link) and the reference feature (from root_link) into a common frame and registers debug visualizations.

tip_link: semantic_digital_twin.world_description.world_entity.KinematicStructureEntity#: The link where the controlled feature is attached. Defines the moving frame of reference.

root_link: semantic_digital_twin.world_description.world_entity.KinematicStructureEntity#: The static reference link. Defines the fixed frame of reference.

controlled_feature: krrood_spatial_math.spatial_types.Point3 | krrood_spatial_math.spatial_types.Vector3#: The geometric feature (point or vector) that is being controlled, expressed in the tip link frame.

reference_feature: krrood_spatial_math.spatial_types.Point3 | krrood_spatial_math.spatial_types.Vector3#: The geometric feature (point or vector) that serves as reference, expressed in the root link frame.

abstract get_controlled_and_reference_features() → tuple[krrood_spatial_math.spatial_types.Point3 | krrood_spatial_math.spatial_types.Vector3, krrood_spatial_math.spatial_types.Point3 | krrood_spatial_math.spatial_types.Vector3]#

Return the controlled and reference features.

Returns:: Tuple (controlled_feature, reference_feature), each a Point3 or Vector3.

build() → giskardpy.motion_statechart.graph_node.MotionNodeArtifacts#

Build the task and derive its observation from its error.

Being within threshold is what this task observes about the world, and succeeding means reaching it, which is what any node is judged by.

class giskardpy.motion_statechart.tasks.feature_functions.AlignPerpendicular#

Bases: FeatureFunctionGoal

Align two normal vectors to be perpendicular.

The goal drives the angle between tip_normal and reference_normal to π/2. Completion occurs when |current_angle - π/2| < threshold.

tip_normal: krrood_spatial_math.spatial_types.Vector3#: The normal vector to be controlled, defined in the tip link frame.

reference_normal: krrood_spatial_math.spatial_types.Vector3#: The reference normal vector to align against, defined in the root link frame.

weight: float#: Priority weight for the alignment constraint in the optimization problem.

maximum_velocity: float = 0.2#: Maximum allowed angular velocity for the alignment motion in radians per second.

threshold: float = 0.01#: Tolerance threshold in radians. The goal is considered achieved when the absolute difference between the current angle and 90 degrees is below this value.

get_controlled_and_reference_features()#

Return the controlled and reference features.

Returns:: Tuple (controlled_feature, reference_feature), each a Point3 or Vector3.

build_artifacts() → giskardpy.motion_statechart.graph_node.MotionNodeArtifacts#

Build a constraint that drives the two normals perpendicular.

Returns:: The artifacts of this task, whose error is how far the dot product of the two normals is from zero.

class giskardpy.motion_statechart.tasks.feature_functions.HeightGoal#

Bases: FeatureFunctionGoal

Moves the tip_point to be the specified distance away from the reference_point along the z-axis of the map frame.

tip_point: krrood_spatial_math.spatial_types.Point3#: Tip point to be controlled.

reference_point: krrood_spatial_math.spatial_types.Point3#: Reference point to measure the distance against.

lower_limit: float#: Lower limit to control the distance away from the reference_point.

upper_limit: float#: Upper limit to control the distance away from the reference_point.

weight: float#: Priority weight for the height constraint in the optimization problem.

maximum_velocity: float = 0.2#: Maximum allowed velocity for the height motion in meters per second.

threshold: float = 0.0#: How far outside the limits still counts as achieved.

get_controlled_and_reference_features()#

Return the controlled and reference features.

Returns:: Tuple (controlled_feature, reference_feature), each a Point3 or Vector3.

build_artifacts() → giskardpy.motion_statechart.graph_node.MotionNodeArtifacts#

Build a constraint that keeps the height difference within the limits.

Returns:: The artifacts of this task, whose error is how far the height difference lies outside the limits.

class giskardpy.motion_statechart.tasks.feature_functions.DistanceGoal#

Bases: FeatureFunctionGoal

Moves the tip_point to be the specified distance away from the reference_point measured in the x-y-plane of the map frame.

tip_point: krrood_spatial_math.spatial_types.Point3#: Tip point to be controlled.

reference_point: krrood_spatial_math.spatial_types.Point3#: Reference point to measure the distance against.

lower_limit: float#: Lower limit to control the distance away from the reference_point.

upper_limit: float#: Upper limit to control the distance away from the reference_point.

weight: float#: Priority weight for the distance constraint in the optimization problem.

maximum_velocity: float = 0.2#: Maximum allowed velocity for the distance motion in meters per second.

threshold: float = 0.0#: How far outside the limits still counts as achieved.

get_controlled_and_reference_features()#

Return the controlled and reference features.

Returns:: Tuple (controlled_feature, reference_feature), each a Point3 or Vector3.

build_artifacts() → giskardpy.motion_statechart.graph_node.MotionNodeArtifacts#

Build a constraint that keeps the planar distance within the limits.

Returns:: The artifacts of this task, whose error is how far the planar distance lies outside the limits.

class giskardpy.motion_statechart.tasks.feature_functions.ReachPoint#

Bases: FeatureFunctionGoal

Drives a controlled point on the tip onto a reference point.

tip_point: krrood_spatial_math.spatial_types.Point3#: The point to be controlled, defined in the tip link frame.

reference_point: krrood_spatial_math.spatial_types.Point3#: The target point to reach, defined in the root link frame.

weight: float#: Priority weight for the reach constraint in the optimization problem.

maximum_velocity: float = 0.2#: Maximum allowed velocity for the reaching motion in meters per second.

threshold: float = 0.01#: Distance at or below which the goal is considered achieved, in meters.

get_controlled_and_reference_features() → tuple[krrood_spatial_math.spatial_types.Point3, krrood_spatial_math.spatial_types.Point3]#

Return the controlled and reference features.

Returns:: Tuple (controlled_feature, reference_feature), each a Point3 or Vector3.

build_artifacts() → giskardpy.motion_statechart.graph_node.MotionNodeArtifacts#

Build a constraint that moves the controlled point onto the reference point.

Returns:: The artifacts of this task, whose error is the distance between the two points.

class giskardpy.motion_statechart.tasks.feature_functions.AngleGoal#

Bases: FeatureFunctionGoal

Controls the angle between the tip_vector and the reference_vector to be between lower_angle and upper_angle.

tip_vector: krrood_spatial_math.spatial_types.Vector3#: Tip vector to be controlled.

reference_vector: krrood_spatial_math.spatial_types.Vector3#: Reference vector to measure the angle against.

lower_angle: float#: Lower limit to control the angle between the tip_vector and the reference_vector.

upper_angle: float#: Upper limit to control the angle between the tip_vector and the reference_vector.

weight: float#: Priority weight for the angle constraint in the optimization problem.

maximum_velocity: float = 0.2#: Maximum allowed angular velocity for the angle motion in radians per second.

threshold: float = 0.0#: How far outside the limits still counts as achieved.

get_controlled_and_reference_features()#

Return the controlled and reference features.

Returns:: Tuple (controlled_feature, reference_feature), each a Point3 or Vector3.

build_artifacts() → giskardpy.motion_statechart.graph_node.MotionNodeArtifacts#

Build a constraint that keeps the angle between the vectors within the limits.

Returns:: The artifacts of this task, whose error is how far the angle lies outside the limits.