storm_kit.differentiable_robot_model.differentiable_robot_model module¶
-
class
DifferentiableRobotModel
(urdf_path: str, learnable_rigid_body_config=None, name='', tensor_args={'device': 'cpu', 'dtype': torch.float32})[source]¶ Bases:
torch.nn.modules.module.Module
Differentiable Robot Model
Initializes internal Module state, shared by both nn.Module and ScriptModule.
-
_is_full_backward_hook
: Optional[bool]¶
-
compute_endeffector_jacobian
(q: torch.Tensor, qd: torch.Tensor, link_name: str) → Tuple[torch.Tensor, torch.Tensor][source]¶
-
compute_fk_and_jacobian
(q: torch.Tensor, qd: torch.Tensor, link_name: str) → Tuple[torch.Tensor, torch.Tensor][source]¶ - Parameters
link_name – name of link name for the jacobian
q – joint angles [batch_size x n_dofs]
qd – joint velocities [batch_size x n_dofs]
Returns: ee_pos, ee_rot and linear and angular jacobian
-
compute_forward_dynamics
(q: torch.Tensor, qd: torch.Tensor, f: torch.Tensor, include_gravity: Optional[bool] = True) → torch.Tensor[source]¶ Computes next qdd by solving the Euler-Lagrange equation qdd = H^{-1} (F - Cv - G - damping_term)
- Parameters
q – joint angles [batch_size x n_dofs]
qd – joint velocities [batch_size x n_dofs]
f – forces to be applied [batch_size x n_dofs]
include_gravity – set to False if your robot has gravity compensation
Returns: accelerations that are the result of applying forces f in state q, qd
-
compute_forward_kinematics
(q: torch.Tensor, qd: torch.Tensor, link_name: str) → Tuple[torch.Tensor, torch.Tensor][source]¶ - Parameters
q – joint angles [batch_size x n_dofs]
link_name – name of link
Returns: translation and rotation of the link frame
-
compute_inverse_dynamics
(q: torch.Tensor, qd: torch.Tensor, qdd_des: torch.Tensor, include_gravity: Optional[bool] = True) → torch.Tensor[source]¶ - Parameters
q – joint angles [batch_size x n_dofs]
qd – joint velocities [batch_size x n_dofs]
qdd_des – desired joint accelerations [batch_size x n_dofs]
include_gravity – when False, we assume gravity compensation is already taken care off
Returns: forces to achieve desired accelerations
-
compute_lagrangian_inertia_matrix
(q: torch.Tensor, include_gravity: Optional[bool] = True) → torch.Tensor[source]¶ - Parameters
q – joint angles [batch_size x n_dofs]
include_gravity – set to False if your robot has gravity compensation
Returns:
-
compute_link_jacobian
(q: torch.Tensor, qd: torch.Tensor, link_name: str) → Tuple[torch.Tensor, torch.Tensor][source]¶ - Parameters
link_name – name of link name for the jacobian
q – joint angles [batch_size x n_dofs]
Returns: linear and angular jacobian
-
compute_non_linear_effects
(q: torch.Tensor, qd: torch.Tensor, include_gravity: Optional[bool] = True) → torch.Tensor[source]¶ Compute the non-linear effects (Coriolis, centrifugal, gravitational, and damping effects).
- Parameters
q – joint angles [batch_size x n_dofs]
qd – [batch_size x n_dofs]
include_gravity – set to False if your robot has gravity compensation
Returns:
-
get_joint_limits
() → List[Dict[str, torch.Tensor]][source]¶ Returns: list of joint limit dict, containing joint position, velocity and effort limits
-
iterative_newton_euler
(base_lin_acc: torch.Tensor, base_ang_acc: torch.Tensor) → None[source]¶ - Parameters
base_lin_acc – linear acceleration of base (for fixed manipulators this is zero)
base_ang_acc – angular acceleration of base (for fixed manipulators this is zero)
-
training
: bool¶
-
update_kinematic_state
(q: torch.Tensor, qd: torch.Tensor) → None[source]¶ Updates the kinematic state of the robot :param q: joint angles [batch_size x n_dofs] :param qd: joint velocities [batch_size x n_dofs]
Returns:
-