vsf.sensor module
- class vsf.sensor.BaseCalibrator[source]
Bases:
objectA base class for sensor calibration. For example, a sensor can be tared or calibrated to a specific range using this class.
- calibrate(sensor: BaseSensor, sim, command_sequence: List[Dict[str, ndarray]], observation_sequence: List[Dict[str, ndarray]]) int[source]
Calibrates the sensor based on the data sequence.
The implementer will use the sensor.set_calibration method, with the appropriate calibration data structure, to set the calibration.
Note that a calibrator could theoretically run the simulation on the entire sequence. However, this is not recommended as it would be using results from the future to calibrate the sensor. It is recommended that only a small fraction of the sequence be used for calibration.
- Parameters:
sensor (BaseSensor) – The sensor to calibrate.
sim (QuasistaticVSFSimulator) – The simulation object.
command_sequence (List[Dict[str,np.ndarray]]) – The command sequence.
observation_sequence (List[Dict[str,np.ndarray]]) – The observation sequence.
- Returns:
The number of time steps used for calibration. Ideally these should be skipped for estimation and prediction.
- Return type:
int
- compatible(sensor: BaseSensor) bool[source]
Returns true if the calibrator is compatible with the sensor.
- class vsf.sensor.BaseSensor[source]
Bases:
objectA sensor model that predicts a real sensor’s observations given a simulation state.
This base class should be extended to implement your sensor model. At minimum, the measurement_names method, and either the predict_torch method or both the predict and measurement_force_jacobian methods must be overridden.
A sensor calibration method can be used to configure how the sensor operates. For example, such a method can tare the sensor, set the measurement range, or set the sensor’s noise model. A BaseCalibrator object with a matching interface should be used to configure the sensor based on the calibration data.
- name
The name of the sensor.
- Type:
str
- attachModelName
The name of the body or robot the sensor is
- Type:
str
- attached to.
- attach(model: klampt.RigidObjectModel | klampt.RobotModelLink | klampt.RobotModel)[source]
Attaches the sensor to a model.
- contact_bodies(state: SimState) List[Tuple[str, str]][source]
Returns a list of bodies that correspond in order with the points predicted via measurement_force_jacobian predictions.
- measurement_errors() List[float][source]
Returns a list of standard deviations for each measurement the sensor provides. If None is returned, then the sensor is assumed to have isotropic noise.
- measurement_force_jacobian(state: SimState) Dict[Tuple[str, str], Tensor][source]
Called to compute the Jacobian of the tactile sensor observation w.r.t. contact forces.
The result is a map from body pairs to Jacobians. A body pair indicates that a body of the sensor is contacting another body and is given in the form (sensor_body,other_body).
A Jacobian must have shape (#meas, N, 3) where N is the number of contact points between the two bodies, 3 is the dimension of the force, and #meas is the number of measurements. Each entry (i,j,k) is the derivative of the i’th measurement with respect to the k’th component of the force (in world coordinates) at the j’th contact point.
NOTE: this function assumes the world state has been configured consistently with the SimState, since SimState does not save the information of robot/object state.
- measurement_names() List[str][source]
Returns a list of names for each measurement the sensor provides.
- predict(state: SimState) Tensor[source]
Returns the sensor’s estimated observation based on the simulation environment. The current attached model is assumed to be updated to the correct configuration.
- predict_torch(state: SimState) Tensor[source]
Returns the sensor’s estimated observation based on the simulation environment. The current attached model is assumed to be updated to the correct configuration.
Should use torch operations to predict the observations in a differentiable way.
- class vsf.sensor.ContactState(points: Tensor, forces: Tensor, elems1: LongTensor | None = None, elems2: LongTensor | None = None)[source]
Bases:
objectA representation of the forces and contact points between two bodies.
- elems1: LongTensor | None = None
- elems2: LongTensor | None = None
- forces: Tensor
- points: Tensor
- to(device) ContactState[source]
- class vsf.sensor.PunyoDenseForceSensor(sensor_name: str, attachModelName: str)[source]
Bases:
BaseSensorA sensor model for the Punyo sensor providing dense force estimates on each vertex of the model’s mesh.
This module simulates dense contact forces at every vertex of a triangle mesh. Each sensor reading is a (3 * num_vertices)-dimensional vector, where entries (3*i, 3*i+1, 3*i+2) represent the local x, y, z force at the i-th vertex in the mesh.
SimState contains contact forces in world coordinates; the sensor internally converts them into local coordinates. For a single body, the sensor’s Jacobian matrix—which maps world-space contact forces to local vertex forces—has dimensions: (3 * num_vertices, num_contact_points, 3). Multiple bodies can be processed by summing their individual contributions.
Two operational modes are supported:
1) Per-Vertex Mode In this simpler mode, each contact point directly corresponds to a single mesh vertex. The local force at vertex i is the sum of all contact forces (transformed to local coordinates) that map to i:
\[\mathbf{F}_{i} = \sum_{c \in C_i} (\mathbf{R}_{\text{local}} \; \mathbf{f}_c^w),\]where \(\mathbf{f}_c^w\) is the c-th contact force in world coordinates, \(\mathbf{R}_{\text{local}}\) is the 3x3 rotation from world frame to the sensor’s local frame, and \(C_i\) is the set of contact points mapped to vertex i.
- Jacobian Construction:
The Jacobian block for vertex i and contact c is \(\mathbf{R}_{\text{local}}\) if contact c is associated with i; otherwise, it is zero. Symbolically:
\[J[3i : 3(i+1),\; c,\; :] = \mathbf{R}_{\text{local}}.\]
2) Barycentric Interpolation Mode In this mode, a contact point c may lie on a triangular (or polygonal) face of the mesh rather than a single vertex. We compute barycentric weights \(w_{i,c}\) so that the force is split among the face’s vertices. The local force at vertex i is:
\[\mathbf{F}_{i} = \sum_{c} w_{i,c} \bigl(\mathbf{R}_{\text{local}} \; \mathbf{f}_c^w\bigr),\]where \(w_{i,c}\) is the barycentric weight of vertex i for contact c.
- Jacobian Construction:
The block for vertex i, contact c is \(w_{i,c} \, \mathbf{R}_{\text{local}}\). Thus:
\[J[3i : 3(i+1),\; c,\; :] = w_{i,c} \, \mathbf{R}_{\text{local}}.\]
In both modes, the sensor aggregates these transformed forces into a single output vector of shape (3 * num_vertices), storing local x, y, z forces per vertex in order. The returned Jacobian is organized as a dense tensor of shape (3 * num_vertices, num_contact_points, 3).
- name
The name of the sensor.
- Type:
str
- attachModelName
The name of the robot link or rigid object that the sensor is attached to.
- Type:
str
- vertices
The vertices of the mesh.
- Type:
np.ndarray
- triangles
The triangles of the mesh.
- Type:
np.ndarray
- attach(model: klampt.RobotModelLink | klampt.RigidObjectModel)[source]
Attaches the sensor to a model.
- measurement_force_jacobian(state: SimState) Dict[Tuple[str, str], Tensor][source]
Result should have shape (3*num_vertices,num_contact_points,3)
- class vsf.sensor.PunyoPressureSensor(name: str, punyo_link: str, base_normal_local: list = [0.0, 0.0, 1.0])[source]
Bases:
BaseSensorPunyo Pressure Sensor Model.
This model produces a one-dimensional observation corresponding to a single pressure reading. The sensor is attached to a triangle mesh representation of the Punyo model, which can be regarded as a thin membrane or shell with a base area to which the mesh is fixed.
Observation Model
The observation model is derived from the force equilibrium equation on the membrane surface. When there is no external contact, the equilibrium can be expressed as:
\[F_{tension} + \iint_{S} p \, d\mathbf{A} = 0,\]where \(p\) is the (constant) pressure acting on the membrane, and the integral is taken over the closed surface \(S\). This integral simplifies because the net flux over a closed surface is equivalent to:
\[\iint_{S} p \, d\mathbf{A} = p \, A_{base} \, \mathbf{n}_{base},\]where \(A_{base}\) is the area of the base of the membrane, and \(\mathbf{n}_{base}\) is its unit normal vector.
After contact occurs, the force equilibrium equation is:
\[F_{tension} + p^{\prime} \, A_{base} \, \mathbf{n}_{base} + \iint_{S} \mathbf{F}_{vsf} \, dA = 0,\]where \(p^{\prime}\) is the new pressure, and \(\mathbf{F}_{vsf}\) represents the contact forces distributed over the surface. Assuming the change in membrane tension is relatively small, we focus on:
\[(p^{\prime} - p) \, A_{base} \, \mathbf{n}_{base} = \iint_{S} \mathbf{F}_{vsf} \, dA.\]Taking the dot product with the unit normal \(\mathbf{n}_{base}\) yields:
\[\begin{split}(p^{\prime} - p) \, A_{base} = \iint_{S} \\bigl(\mathbf{F}_{vsf} \cdot \mathbf{n}_{base}\\bigr) \, dA.\end{split}\]Consequently,
\[\begin{split}(p^{\prime} - p) = \\frac{1}{A_{base}} \iint_{S} \\bigl(\mathbf{F}_{vsf} \cdot \mathbf{n}_{base}\\bigr) \, dA.\end{split}\]In practice, this relationship gives a simple way to compute the pressure deviation based on the integrated contact forces normal to the membrane.
- name
The name of the sensor.
- Type:
str
- attachModelName
The name of the Punyo model to which the sensor is attached.
- Type:
str
- object
The Punyo model object associated with this sensor.
- Type:
RigidObjectModel
- vertices
The array of vertex coordinates for the Punyo model, of shape (N, 3).
- Type:
np.ndarray
- triangles
The array of triangle indices for the Punyo model, of shape (M, 3).
- Type:
np.ndarray
- vertex_normals
The normals at each vertex, of shape (N, 3).
- Type:
torch.Tensor
- vertex_areas
The area corresponding to each vertex (e.g., the area of its Voronoi region on the mesh), of shape (N, 1).
- Type:
torch.Tensor
- scaled_vertex_normals
The vertex normals scaled by their corresponding area, of shape (N, 3).
- Type:
torch.Tensor
- attach(model: klampt.RigidObjectModel | klampt.RobotModelLink)[source]
Attaches the sensor to a punyo model.
- measurement_force_jacobian(state: SimState) Dict[Tuple[str, str], Tensor][source]
Measure the Jacobian of the pressure sensor observation w.r.t. contact forces. The result is a map from body pairs to Jacobians. A body pair indicates that a body of the sensor is contacting another body and is given in the form (sensor_body,other_body).
A Jacobian must have shape (1,N,3) where N is the number of contact points between the two bodies, 3 is the dimension of the force, and 1 is the dimension of the pressure measurement.
The Jacobian is computed as the derivative of the pressure with respect to the contact forces at the contact points. Each entry (i,j,k) is the derivative of the pressure with respect to the k’th component of the force (in world coordinates) at the j’th contact point. The Jacobian is computed as the product of the base normal vector and the contact force vector, divided by the base area, for each contact point between the sensor and the other body.
- Parameters:
state (SimState) – The current simulation state.
- Returns:
A dictionary mapping body pairs to Jacobians.
- Return type:
Dict[Tuple[str,str],torch.Tensor]
- predict(state: SimState) Tensor[source]
Called to update the sensor based on the simulation environment.
- class vsf.sensor.SimState(body_transforms: ~typing.Dict[str, ~torch.Tensor] = <factory>, body_states: ~typing.Dict[str, ~torch.Tensor] = <factory>, contacts: ~typing.Dict[~typing.Tuple[str, str], ~vsf.sensor.base_sensor.ContactState] = <factory>)[source]
Bases:
objectA representation of the simulator state used for sensor simulation.
Forces are not assumed to be symmetric, so if you wish to store contact forces applied to a with equal and opposite forces on b, you must store both (a,b) and (b,a) in the contact_forces dictionary.
- bodies_in_contact(name: str, reverse=False) List[str][source]
Returns the list of bodies that name is in contact with (or that are in contact with name if reverse=True).
- body_states: Dict[str, Tensor]
Named body states (e.g., robot configs, deformable states)
- body_transforms: Dict[str, Tensor]
Named rigid body homogeneous transforms
- contact_states_on_body(name: str, reverse=False) List[ContactState][source]
Returns a list of contact states affecting a given body.
These will match the order given by bodies_in_contact(name,reverse).
If reverse = False, all the forces are affecting the body. Otherwise, all the forces are affected by the body.
- contacts: Dict[Tuple[str, str], ContactState]
Contact state between pairs of rigid bodies
- contacts_on_body(name: str, local: bool = True) Tuple[Tensor, Tensor][source]
Returns a concatenated list of all contacts affecting a given body.
- Parameters:
name (str) – the name of the body
local (bool, optional) – if True, returns the contact points and forces in local coordinates. Defaults to True.
- Returns:
the contact points and forces
- Return type:
Tuple[torch.Tensor,torch.Tensor]
- property device
- property dtype
- class vsf.sensor.TareCalibrator(num_samples: int = 10, output_key: str = 'tare')[source]
Bases:
BaseCalibratorA simple calibrator that tares the sensor based on the average of some initial set of observations.
NOTE: the average is not average observation, but the average of the observation residual after subtracting simulation with empty VSF model.
The calibration has the form {output_key: average_observation}
- calibrate(sensor, sim, command_sequence, observation_sequence) int[source]
Calibrates the sensor based on the data sequence.
The implementer will use the sensor.set_calibration method, with the appropriate calibration data structure, to set the calibration.
Note that a calibrator could theoretically run the simulation on the entire sequence. However, this is not recommended as it would be using results from the future to calibrate the sensor. It is recommended that only a small fraction of the sequence be used for calibration.
- Parameters:
sensor (BaseSensor) – The sensor to calibrate.
sim (QuasistaticVSFSimulator) – The simulation object.
command_sequence (List[Dict[str,np.ndarray]]) – The command sequence.
observation_sequence (List[Dict[str,np.ndarray]]) – The observation sequence.
- Returns:
The number of time steps used for calibration. Ideally these should be skipped for estimation and prediction.
- Return type:
int