Utilities¶

AGX Utilities¶

gym_agx.utils.agx_utils.save_simulation(sim, file_name, aagx=False)¶

Save AGX simulation object to file.

Parameters

sim (agxSDK.Simulation) -- AGX simulation object
file_name (str) -- name of the file
aagx (bool) -- additionally saves human-readable markup file

Returns

Boolean for success/failure

gym_agx.utils.agx_utils.add_goal_assembly_from_file(sim, file_path)¶

Adds goal_assembly to simulation, by loading file_name. It disables collisions between start_assembly and goal_assembly.

Parameters

sim (agxSDK.Simulation) -- AGX simulation object
file_path (str) -- path to file to load

gym_agx.utils.agx_utils.make_goal_simulation(sim, remove_assemblies=[])¶

Adds the suffix goal to all objects in the simulation and makes them static. Removes undesired assemblies.

Parameters

sim (agxSDK.Simulation) -- AGX simulation object
remove_assemblies (list) -- string list of assemblies to remove

gym_agx.utils.agx_utils.to_numpy_array(agx_list)¶

Convert from AGX data structure to NumPy array.

Parameters: agx_list -- AGX data structure
Returns: NumPy array

gym_agx.utils.agx_utils.to_agx_list(np_array, agx_type)¶

Convert from Numpy array to AGX data structure.

Parameters

np_array (numpy.ndarray) -- NumPy array
agx_type -- Target AGX data structure

Returns

AGX data object

gym_agx.utils.agx_utils.get_cable_segment_edges(cable)¶

Get AGX Cable segments' begin and end positions.

Parameters: cable (agxCable.Cable) -- AGX Cable object
Returns: NumPy array with segments' edge positions

gym_agx.utils.agx_utils.create_body(shape, name='', position=<agx.Vec3; proxy of <Swig Object of type 'agx::Vec3T< double > *'> >, rotation=<agx.OrthoMatrix3x3; proxy of <Swig Object of type 'agx::OrthoMatrix3x3 *'> >, geometry_transform=<agx.AffineMatrix4x4; proxy of <Swig Object of type 'agx::AffineMatrix4x4 *'> >, motion_control=3, material=None, disable_collisions=False)¶

Helper function that creates a RigidBody according to the given definition. Returns the body itself, it's geometry and the OSG node that was created for it.

Parameters

shape (agxCollide.Shape) -- shape of object
name (string) -- Optional. Defaults to "". The name of the new body
position (agx.Vec3) -- Position of the object in world coordinates
rotation (agx.OrthoMatrix3x3) -- Rotation of the object in world coordinate frames
geometry_transform (agx.AffineMatrix4x4) -- Optional. Defaults to identity transformation. The local transformation of the shape relative to the body
motion_control (agx.RigidBody.MotionControl) -- Optional. Defaults to DYNAMICS
material (agx.Material) -- Ignored if not given. Material assigned to the geometry created for the body
disable_collisions (bool) -- Optional. Disable geometry collisions

Returns

assembly

gym_agx.utils.agx_utils.create_ring(name, radius, element_shape, num_elements, constraint_type=<class 'agx.LockJoint'>, rotation_shift=0, translation_shift=0, compliance=None, material=None, center=<agx.Vec3; proxy of <Swig Object of type 'agx::Vec3T< double > *'> >, normal=<agx.Vec3; proxy of <Swig Object of type 'agx::Vec3T< double > *'> >)¶

Creates a Ring object.

Parameters

name (string) -- name of ring object as a string
radius (float) -- radius of the ring circumference, centered on the composing elements
element_shape (agxCollide.Shape) -- AGX shape type to be used as building block of ring
num_elements (int) -- number of elements of element_type which will be used to construct ring
constraint_type (agx.Constraint) -- type of constraint that should connect each element
rotation_shift (float) -- positive rotation around z axis of seed element (radians). Useful for shapes which are initialized along axis other than x
translation_shift (float) -- translation of constraints, off the center of mass, along y-axis of the object
compliance (list) -- compliance of constraints along 6DOF linking each element of the ring
center (agx.Vec3) -- position in world coordinates of the center of the ring
material (agx.Material) -- material the ring elements are made of
normal (agx.Vec3) -- unit vector placed at the center position to define plane where the ring should lie

:return assembly

gym_agx.utils.agx_utils.create_ring_element(name, element_shape, material)¶

Creates single ring element based on shape and material.

Parameters

name (string) -- name of ring element
element_shape (agxCollide.Shape) -- enum value of ring geometric shape
material (agx.Material) -- material of rigid body

:return ring_element

gym_agx.utils.agx_utils.create_locked_prismatic_base(name, rigid_body, compliance=0, damping=0.3333333333333333, position_ranges=None, motor_ranges=None, locked_at_zero_speed=None, lock_status=None, compute_forces=False, radius=0.005, length=0.05)¶

Creates a prismatic, collision free, base object and attaches a rigid body to it, via LockJoint.

Parameters

name (string) -- name of prismatic base object as a string
rigid_body (agx.RigidBody) -- AGX rigid body object which should be attached to prismatic base
compliance (float) -- compliance of the LockJoint which attaches the rigid body to the base
damping (float) -- damping of the LockJoint which attaches the rigid body to the base
position_ranges (list) -- a list containing three tuples with the position range for each constraint (x,y,z)
motor_ranges (list) -- a list of three tuples with the position range for each constraint (x,y,z)
locked_at_zero_speed (list) -- a list of three bools indicating zero speed behaviour each constraint (x,y,z,2rb)
lock_status (list) -- a list of boolean values for whether to activate the constraint locks (x,y,z)
compute_forces (boolean) -- set whether forces are computed for this base
radius (float) -- radius of the cylinders making up the base. For visualization purposes only
length (float) -- length of the cylinders making up the base. For visualization purposes only

:return assembly

gym_agx.utils.agx_utils.create_hinge_prismatic_base(name, rigid_body, compliance=0, damping=0.3333333333333333, position_ranges=None, motor_ranges=None, locked_at_zero_speed=None, lock_status=None, axis=<agx.Vec3; proxy of <Swig Object of type 'agx::Vec3T< double > *'> >, compute_forces=False, radius=0.005, length=0.05)¶

Creates a prismatic, collision free, base object and attaches a rigid body to it, via Hinge.

Parameters

name (string) -- name of prismatic base object as a string
rigid_body (agx.RigidBody) -- AGX rigid body object which should be attached to prismatic base
compliance (float) -- compliance of the Hinge which attaches the rigid body to the base
damping (float) -- damping of the Hinge which attaches the rigid body to the base
position_ranges (list) -- a list containing three tuples with the position range for each constraint (x,y,z,rb)
motor_ranges (list) -- a list of three tuples with the position range for each constraint (x,y,z,rb)
locked_at_zero_speed (list) -- a list of three bools indicating zero speed behaviour each constraint (x,y,z,2rb)
lock_status (list) -- a list of boolean values for whether to activate the constraint locks (x,y,z)
axis (agx.Vec3) -- vector determining axis of rotation of rigid body
compute_forces (boolean) -- set whether forces are computed for this base
radius (float) -- radius of the cylinders making up the base. For visualization purposes only
length (float) -- length of the cylinders making up the base. For visualization purposes only

:return assembly

gym_agx.utils.agx_utils.create_universal_prismatic_base(name, rigid_body, compliance=0, damping=0.3333333333333333, position_ranges=None, motor_ranges=None, locked_at_zero_speed=None, lock_status=None, compute_forces=False, radius=0.005, length=0.05)¶

Creates a prismatic, collision free, base object and attaches a rigid body to it, via UniversalJoint. Note that at this time, the UniversalJoint constraint has known issues. I should be avoided if possible.

Parameters

name (string) -- name of prismatic base object as a string
rigid_body (agx.RigidBody) -- AGX rigid body object which should be attached to prismatic base
compliance (float) -- compliance of the UniversalJoint which attaches the rigid body to the base
damping (float) -- damping of the UniversalJoint which attaches the rigid body to the base
position_ranges (list) -- a list containing three tuples with the position range for each constraint (x,y,z,2rb)
motor_ranges (list) -- a list of three tuples with the force range for each constraint (x,y,z,2rb)
locked_at_zero_speed (list) -- a list of three bools indicating zero speed behaviour each constraint (x,y,z,2rb)
lock_status (list) -- a list of boolean values for whether to activate the constraint locks (x,y,z)
compute_forces (boolean) -- set whether forces are computed for this base
radius (float) -- radius of the cylinders making up the base. For visualization purposes only
length (float) -- length of the cylinders making up the base. For visualization purposes only

:return assembly

gym_agx.utils.agx_utils.create_prismatic_base(name, radius, length, compute_forces, position_ranges, motor_ranges, locked_at_zero_speed, lock_status)¶

Creates a prismatic, collision free, base object and attaches a rigid body to it, via PrismaticJoint.

Parameters

name (string) -- name of prismatic base object as a string
radius (float) -- radius of the cylinders making up the base. For visualization purposes only
length (float) -- length of the cylinders making up the base. For visualization purposes only
compute_forces (bool) -- set whether forces are computed for this base
position_ranges (list) -- a list of three tuples with the position range for each constraint (x,y,z,2rb)
motor_ranges (list) -- a list of three tuples with the force range for each constraint (x,y,z,2rb)
locked_at_zero_speed (list) -- a list of three bools indicating zero speed behaviour each constraint (x,y,z,2rb)
lock_status (list) -- a list containing boolean values for whether to activate the constraint locks (x,y,z)

:return assembly

General Utilities¶

gym_agx.utils.utils.construct_space(observation, inc=0)¶

General purpose function to construct OpenAI Gym spaces from a sampled observation.

Parameters

observation -- sampled observation, made up of nested dictionaries which have NumPy arrays at leaves
inc -- helps determine number of recursive calls (should not be manually set)

Returns

OpenAI Gym space

gym_agx.utils.utils.goal_area(achieved_goal, goal)¶

Computes area between desired goal and achieved goal.

Parameters

achieved_goal -- vector of achieved goal
goal -- vector of desired goal

Returns

area

gym_agx.utils.utils.rotate_rpy(input_vector, phi, theta, psi, transpose=False)¶

Apply rotation to vector using Roll-Pitch-Yaw (RPY) or ZYX angles.

Parameters

input_vector (numpy.ndarray) -- input vector
phi -- roll angle around z
theta -- pitch angle around y
psi -- yaw angle around x
transpose (bool) -- Boolean variable that toggles transpose rotation

:return numpy.ndarray

gym_agx.utils.utils.sample_sphere(center, radius_range, polar_range, azimuthal_range, rpy_angles=None)¶

Uniform sampling bounded by sphere. Default spherical coordinate frame is same as AGX Dynamics base frame and the Roll-Pitch-Yaw (RPY) or ZYX angles are used to rotate the points generated in this frame.

Parameters

center -- center position of the sphere
radius_range -- range of acceptable radius values [min, max]
polar_range -- range of values for polar angle (xy-z) in [0, pi], angle=0 aligned with z-axis
azimuthal_range -- range of values for azimuthal angle (xy plane) in [0, 2*pi], angle=0 aligned with x-axis
rpy_angles -- RPY angles (phi - roll around z, theta - pitch around y, psi - yaw around x)

:return position, radius

gym_agx.utils.utils.point_to_point_trajectory(t, time_limit, start_position, end_position, degree=3)¶

Assuming a third/fifth order polynomial trajectory: x(s) = x_start + s(x_end - x_start) , s in [0, 1] with time scaling: s(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 , t in [0, T].

Parameters

t -- time t, for instant velocity
time_limit -- time limit, independent of start_time. Duration of trajectory
start_position -- position at start_time
end_position -- position at start_time + time_limit
degree -- (optional) degree of polynomial (3 or 5)

Returns

instant velocity

gym_agx.utils.utils.polynomial_trajectory(current_time, start_time, waypoints, time_scales, degree=3)¶

Third/fifth order polynomial trajectory: x(s) = waypoints[i] + s(waypoints[i+1] - waypoints[i]), s in [0,1] with time scaling: s(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 , t in [0, T].

Parameters

current_time -- current time
start_time -- provides reference for start of whole trajectory
waypoints -- list of Numpy arrays with waypoints including start and end positions
time_scales -- list with desired timescale between consecutive waypoints
degree -- (optional) degree of polynomial (3 or 5)

Returns

instant velocity

gym_agx.utils.utils.find_reference_angle(angle)¶

Finds reference angle in first quadrant.

Parameters: angle -- angle in radians
Returns: reference angle and sign

gym_agx.utils.utils.compute_linear_distance(v0, v1)¶

Computes linear distance between two points.

Parameters

v0 -- NumPy array
v1 -- NumPy array

Returns

Euclidean distance between v0 and v1

gym_agx.utils.utils.compute_angle(v0, v1)¶

Computes angle between two segments (through circumscribed osculating circle).

Parameters

v0 -- NumPy array
v1 -- NumPy array

Returns

angle in radians

gym_agx.utils.utils.compute_curvature(v0, v1, segment_length=1)¶

Computes curvature between two segments (through circumscribed osculating circle).

Parameters

v0 -- NumPy array
v1 -- NumPy array
segment_length -- length of AGX Cable segment (default 1)

Returns

a positive scalar corresponding to the curvature: K = 2*tan(tangent_angle/2) / segment_length

gym_agx.utils.utils.get_cable_angles(cable_segment_edges)¶

Iterates through cable state to compute angle between three adjacent points.

Parameters: cable_segment_edges -- Numpy array with coordinates of cable segments

gym_agx.utils.utils.get_cable_curvature(cable_segment_edges, segment_length=1)¶

Iterates through cable state to compute curvature between three adjacent points.

Parameters

cable_segment_edges -- Numpy array with coordinates of cable segments
segment_length -- length of AGX Cable segment (default 1)

gym_agx.utils.utils.compute_torsion(v0, v1, v2, segment_length=1)¶

Computes torsion between two segments (through circumscribed osculating circle).

Parameters

v0 -- NumPy array
v1 -- NumPy array
v2 -- NumPy array
segment_length -- length of AGX Cable segment (default 1)

Returns

a positive scalar corresponding to the curvature: T = 2*tan(binormal_angle/2) / segment_length

gym_agx.utils.utils.get_cable_torsion(cable_state, segment_length=1)¶

Iterates through cable state to compute torsion between four adjacent points.

Parameters

cable_state -- Numpy array with coordinates of cable segments
segment_length -- length of AGX Cable segment (default 1)

gym_agx.utils.utils.point_inside_polygon(polygon, point)¶

Point in polygon algorithm (Jordan theorem).

Parameters

polygon --
point --

Returns

gym_agx.utils.utils.all_points_below_z(points, max_z)¶

Test if all segments are below a certain height.

Parameters

points --
max_z --

Returns