vsf.sim module

class vsf.sim.NeuralVSFQuasistaticSimBody(vsf: NeuralVSF, config: NeuralVSFSimConfig)[source]

Bases: object

A class that simulates the quasistatic behavior of a body using NeuralVSF.

As object points are dragged through the volume, we record the forces on each point and integrate them to get the final force on the object.

TODO: implement this in recursive form.

property device
property dtype
load_state(state: dict)[source]
reset()[source]

Setup contact information recorder and force integrators.

This function can be called to clear force integration history of NeuralVSF. Update self.vertex_trajectory_local: dict[str, torch.Tensor], keys are object name and save all triangle mesh vertices trajectory in contact with NeuralVSF in the VSF local frame. self.vertex_normal_local: dict[str, torch.Tensor], keys are object name and save all vertices normal corresponding to the vertices trajectory in the VSF local frame.

state()[source]
step(state: klamptWorldWrapper, dt: float) → Tuple[ndarray, ndarray, Tensor, Tensor][source]

This function runs NeuralVSF simulation based on the control sequence.

Returns:

contains the object index, element index, contact point, and contact force (world coordinates) for each contact.

Return type:

obj_index, obj_elems, cps, forces

class vsf.sim.PointVSFQuasistaticSimBody(vsf_model: PointVSF, contact_params: ContactParams, triangle_mesh: klampt.model.geometry.TriangleMesh | TriangleMesh | None = None)[source]

Bases: object

Stores information to simulate quasistatic interaction with a point VSF.

A stick-slip model is used to simulate sliding contacts. Each point that penetrates another object first gets associated with an anchor point on that object.

vsf_model

The point VSF model.

pose

The current pose of the body.

contact_params

The contact parameters.

perfer

The performance recorder.

verbose

Whether to print debug information.

obj_knn

The nearest neighbor search for the object.

point_object_idx

A boolean array indicating which VSF points are in contact.

point_local

The deformed position of each point in the VSF, in VSF local coordinates.

anchor_local

The anchor position of each point in the VSF, in other-object local coordinates.

anchor_normal_local

The anchor normal of the contacted location, pointing outward, in other-object local coordinates.

deformed_points(world=True) → ndarray[source]

Returns all the points of the VSF, deformed by the current state

deformed_points_torch(world=True) → Tensor[source]

Returns all the points of the VSF, deformed by the current state

property device

Get the device of the VSF model

property dtype

Get the dtype of the VSF model

get_deform_motion_lines(slide_color=[0.0, 1.0, 0.0], world=False, verbose=False) → Tuple[PointCloud, PointCloud, LineSet][source]

Returns the motion lines of the deformed points

Parameters:

  • slide_color – the color of the sliding points

  • world – whether to return the motion lines in world coordinates

  • verbose – whether to print debug information

Returns:

the rest point cloud curr_pcd: the current point cloud motion_lines: the motion lines

Return type:

rest_pcd

get_obj_pcd(color_method: str = 'contact', dtype='o3d')[source]

Get the object point cloud with color based on contact status

get_proxy_pts(query_pts: ndarray, prox_margin=0.01, max_nn_pts=20) → tuple[ndarray, ndarray][source]

Find VSF points that are close to the query points.

NOTE: this only uses the REST points. Should we use the deformed points?

Parameters:

  • query_pts – query points in world coordinates, shape (N, 3)

  • prox_margin – margin for proximity search

  • max_nn_pts – maximum number of nearest neighbors to search

Returns:

  • prox_idx_ary: indices of the VSF points that are close to the query points

  • prox_pts: VSF points that are close to the query points, in world space

load_state(state: dict)[source]

Load the simulation state of the point VSF body from a dictionary

Parameters:

  • state – the dictionary containing the pose, point object index,

  • local (and anchor normal)

  • local

  • local

property num_contacts

Get the number of VSF points in contact

reset()[source]

Reset the simulation state of point VSF.

Clear all contact points and anchor points.

setup_obj_knn()[source]

Setup the nearest neighbor search for the object.

This is used to find the VSF points that are close to the query points.

slide_contacts(state: klamptWorldWrapper, modify_data: bool = False)[source]

Perform stick/slide logic for the VSF points in contact with the object.

For points that are in contact, check if they are sticking or sliding. Stick/slide condition is determined whether the contact force direction is within the friction cone. If the point is sticking, do nothing, kinematic update will let the point move with the object. If the point is sliding, query the contact point on the object and update the contact point. If the contact force direction points outward the surface, remove the contact point.

Parameters:

  • state – the current state of the world

  • modify_data – whether to modify the contact data

state()[source]

Get the current state of the point VSF body

TODO: converts the state to a dataclass

Returns:

A dictionary containing the pose, point object index, point local, anchor local, and anchor normal local

step(state: klamptWorldWrapper, dt: float) → Tuple[ndarray, ndarray, ndarray, Tensor][source]

Steps the simulation. Returns the set of contact indices, bodies, points, and forces

The step function in point VSF performs the following steps: 1. detect stick/slide contacts, this step will keep sticking points, slide points on the contact surface, and remove points whose contact force points outward the surface. 2. get new set of points in contact with the object, using query_point_contacts function, when using proxy method, the query points are the VSF points that are close to the object. 3. compute the force tensor for the points in contact, using the VSF model.

Parameters:

  • state – the current state of the world

  • dt – the time step, the number is not used since running quasistatic simulation

TODO: convert the return values to a dataclass

Returns:

The set of contact indices, bodies, points, and forces indices: the indices of the VSF points in contact bodies: the indices of the objects that the VSF points are in contact with points: the contact points in world coordinates forces: the contact forces in world coordinates

class vsf.sim.QuasistaticVSFSimulator(klampt_world_wrapper: klamptWorldWrapper, sensors: list[BaseSensor])[source]

Bases: object

A simulator for Volumetric Stiffness Fields that assumes quasistatic, stick-slip contact with PointVSF objects and sticking-only contact with NeuralVSF objects.

Usage:

world = klamptWorldWrapper(klampt_world)
robot = klampt_world.robot(0)
sensors = [JointTorqueSensor('torques',robot.name)]
sim = QuasistaticVSFSimulator(world, sensors)
sim.add_deformable('vsf_name',vsf)
dt = 0.1
for i in range(30):
    config = robot.getConfig()
    config[3] += dt * 1.0
    control = {robot.name:config}  #moves the robot's 3rd joint
    sim.step(control, dt)
    print(sim.measurements())
    #print(sim.state())
    #print(sim.measurement_jacobians())
klampt_world_wrapper

the Klamp’t world wrapper

Type:

klamptWorldWrapper

vsf_objects

the VSF objects

Type:

dict[str,Union[PointVSF,NeuralVSF]]

sensors

the sensors attached to the simulator

Type:

list[BaseSensor]

perfer

a performance recorder, default DummyRecorder

Type:

PerfRecorder

add_deformable(name: str, vsf: PointVSF | NeuralVSF, contact_params: ContactParams | None = None) → PointVSFQuasistaticSimBody | NeuralVSFQuasistaticSimBody[source]
get_control_keys(suffix='_state') → dict[str, str][source]

Returns the control keys of the simulator.

Parameters:

suffix – String to be added after the object name, default ‘_state’.

get_sensor(name: str) → BaseSensor[source]

Returns the sensor with the given name, return None if the sensor does not exist.

Inputs: - name (str): the name of the sensor

Outputs: - sensor (BaseSensor): the sensor object

get_sensor_keys(suffix='') → dict[str, str][source]

Returns the sensor keys of the simulator.

Parameters:

suffix – String to be added after the sensor name, default ‘’.

load_state(state: SimState)[source]

Load the internal state of the simulator.

load_state_dict(state: dict)[source]

This function loads the simulator state from a dictionary previously returned by state_dict().

measurements() → dict[str, Tensor][source]

Returns the simulated noise-free measurements at the current state

reset()[source]

This function resets the simulator state to zero state.

set_perfer(perfer: PerfRecorder)[source]
state() → SimState[source]

Return the internal state of the simulator.

state_dict() → dict[source]

This function returns the current state of the simulator.

The dictionary has strings as keys and can contain lists, numpy arrays, and torch.Tensors as values.

step(controls: dict[str, ndarray], dt: float)[source]

Simulates the state transition based on the control input.

Parameters:

  • controls (dict[str, np.ndarray]) – A dictionary containing the control inputs. The keys are the names of the control inputs. Each control input is a NumPy array of shape (*controlInputShape). For example, with a single robot arm in the simulator, the control input shape is (numJoints,).

  • dt (float) – The time step for the simulation.

class vsf.sim.klamptWorldWrapper[source]

Bases: object

A wrapper for the Klamp’t world.

Stores list of bodies in a flat dict, and gives an ordering to the names of objects in the world. PCDs and SDFs are stored with each geometry so that you can query contact points more quickly.

add_geometry(name: str, geom: klampt.Geometry3D, geom_type: str, parent_name: str | None = None, parent_relative_transform: ndarray | None = None)[source]

Add a geometry to the klamp’t world

Parameters:

  • name – name of the geometry

  • geom – geometry object

  • geom_type – type of the geometry, “rigid” or “deformable”

  • parent_name – name of the parent geometry

add_geometry_from_file(name: str, geom_file_name: str, geom_type: str, parent_name: str | None = None, parent_relative_transform: ndarray | None = None)[source]

Add a geometry to the klamp’t world

Parameters:

  • name – name of the geometry

  • geom_file_name – file name of the geometry

  • geom_type – type of the geometry, “rigid” or “deformable”

  • parent_name – name of the parent geometry

  • parent_relative_transform – relative transformation between the geometry and its parent

add_robot(name: str, robot_file_name: str)[source]

Add a robot to the klamp’t world

Parameters:

  • name – name of the robot

  • robot_file_name – URDF file name of the robot

apply_control(name: str, control: ndarray)[source]

Apply control to an object in the world

Parameters:

  • name – name of the object

  • control – control array for the object

convert_geometry(geom_idx_lst: list[int], src_type: str, tgt_type: str)[source]

Convert the geometry type of the rigid objects in the Klamp’t world

Parameters:

  • geom_idx_lst – list of geometry indices

  • src_type – source geometry type

  • tgt_type – target geometry type

“src_type” and “tgt_type” can be one of the following: - ‘ImplicitSurface’ - ‘TriangleMesh’ - ‘PointCloud’ Note this function will not change geometry not matching the “src_type”

find_closest_point(query_points: ndarray, geom_idx: int | None = None, verbose: bool = False) → tuple[ndarray, ndarray][source]

Find the closest point from a set of query points to a geometry in the Klamp’t world. The geometry is given by index geom_idx.

TODO: currently this function only supports points query in a for loop, should be substituted by batch query in the future.

Inputs: :param query_points: a Nx3 numpy array of query points :param geom_idx: index of the geometry in the Klamp’t world :param verbose: whether to visualize the query results

Outputs: :return: a tuple of closest points and normals

static from_world(world: klampt.WorldModel) → klamptWorldWrapper[source]

Create a Klamp’t world wrapper from an existing Klamp’t world

get_all_pcd(format='open3d')[source]

Get all point clouds of the objects in the Klamp’t world. All point clouds are transformed in the world coordinate frame.

Parameters:

format – format of the point cloud, ‘open3d’ or ‘numpy’

get_geom_trans_dict(mode='local2world', format='klampt') → dict[str, ndarray][source]

Get the transformation of all geometries in the Klamp’t world

get_geometry(name: str) → klampt.Geometry3D[source]

Get the geometry of an object in the Klamp’t world

Inputs: :param name: name of the object

Outputs: :return: the geometry of the object

get_transform(model: klampt.RigidObjectModel | klampt.RobotModelLink, mode='local2world', format='klampt')[source]

Get the rigid transformation of a geometry in the Klamp’t world Here the model can be a rigid object or a robot link, we can get transformation from world to local or local to world. The return type can be Klamp’t SE3 transformation or 4x4 homogeneous numpy transformation matrix.

Parameters:

  • model – the model of the geometry

  • mode – transformation mode, ‘local2world’ or ‘world2local’

  • format – transformation format, ‘klampt’ or ‘numpy’

Returns:

the transformation matrix, in user specified format

property num_bodies
query_point_contacts(query_points: ndarray | klampt.Geometry3D, padding1: float = 0.001, padding2: float = 0.001, body_idx_lst=None) → tuple[ndarray, ndarray, ndarray, ndarray][source]

Query whether the points are in contact with any objects in the Klamp’t world.

This function uses the geometry.contacts function to query the contact points, which applies the boundary layer method for collision checking.

TODO: The collision query result should be saved in a specific dataclass to make the query results more readable.

Parameters:

  • query_points (np.ndarray | klampt.Geometry3D) – A (N,3) NumPy array of query points.

  • padding1 (float, optional) – Padding of the query points. Default is 1e-3.

  • padding2 (float, optional) – Padding of the geometry. Default is 1e-3.

  • body_idx_lst (list, optional) – List of body indices to query. If None, all bodies are queried.

Returns:

A tuple containing:

  • body_idx (np.ndarray): Indices of bodies in contact, or -1 if no contact.

  • depths (np.ndarray): Penetration depths.

  • contact_pts (np.ndarray): Closest points on body surfaces (in world coordinates).

  • contact_nms (np.ndarray): Contact normals (in world coordinates).

The length of these arrays is the same as the number of query points.

Return type:

tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]

setup_local_pcd_lst(sample_backend='klampt', dispersion=0.01, num_of_pts=1000)[source]

Setup the open3d point cloud list, save in the self.local_pcd_lst list. The point clouds are used for proximity queries to detect contact points with PointVSF.

NOTE: the dispersion in the point cloud sampling should match the resolution of PointVSF to ensure accurate contact detection. Too coarse point cloud may miss contacts, while too fine point cloud may slow down the simulation.

TODO: this function should be substituted by multi-geometry support in Klamp’t, ideally should be saved in a structure accssible in the RigidObjectModel or RobotModelLink object.

Parameters:

  • sample_backend – the backend to sample the point cloud, ‘klampt’ or ‘open3d’

  • dispersion – dispersion of the point cloud, only used when sample_backend is ‘klampt’

  • num_of_pts – number of points in the point cloud, only used when sample_backend is ‘open3d’

setup_local_sdf_lst(resolution=0.01)[source]

Setup the the signed distance field list of all geometries in the Klamp’t world. All SDFs are in the geometry local frame, saved in the self.local_sdf_lst list.

NOTE: this function should be substituted by multi-geometry support in Klamp’t, ideally should be saved in a structure accssible in the RigidObjectModel or RobotModelLink object.

Parameters:

resolution – resolution of the signed distance field

update_attachments()[source]

Automatically update objects attached to kinematically controlled objects. This function should only be called internally.

NOTE: this is a workaround for the lack of proper attachment support in Klamp’t, ideally should be substituted by automatic kinematic update in Klamp’t.

update_deformable(name: str, control: ndarray)[source]

This function update the triangle meshes of a deformable object in the Klamp’t world

TODO: Currently only update the triangle mesh, should trigger SDF/point cloud update in the future.

Parameters:

  • name – name of the deformable object

  • control – control should be Nx3 vertex positions

update_geometry(name: str, rotation: klampt.model.typing.Rotation, position: klampt.model.typing.Vector3)[source]

Update the rigid transformation of a geometry

Parameters:

  • name – name of the geometry

  • rotation – new rotation, should be in Klampt Rotation format

  • position – new translation, should be in Klampt Vector3 format

update_robot(name: str, robot_drivers: ndarray)[source]

Update the configuration of a robot in the Klamp’t world

Parameters:

  • name – name of the robot

  • robot_config – new configuration of the robot