pyrcf.utils.kinematics_dynamics =============================== .. py:module:: pyrcf.utils.kinematics_dynamics .. autoapi-nested-parse:: Utility functions and classes for getting robot dynamics and kinematics data. Submodules ---------- .. toctree:: :maxdepth: 1 /autoapi/pyrcf/utils/kinematics_dynamics/pinocchio_interface/index Classes ------- .. autoapisummary:: pyrcf.utils.kinematics_dynamics.PinocchioInterface Package Contents ---------------- .. py:class:: PinocchioInterface(urdf_filename: str, floating_base: bool = True, verbose: bool = True, ee_names: List[str] = None) Bases: :py:obj:`pinocchio.RobotWrapper` A pinocchio.RobotWrapper extension to get robot kinematics and dynamics values easily. This class cannot deal with passive joints, spherical joints or other composite joints. It works fine with regular revolute and prisimatic joints, as well as handles continuous joints. .. py:method:: configuration_handle_continuous_joints(q: numpy.ndarray, continuous_joint_q_ids: Sequence[float]) -> numpy.ndarray :staticmethod: Static method to update the provided pinocchio q vector to handle continuous joints. This is to be used when the original indices are filled with the joint position values. These will then be replaced with cos(theta) and sin(theta) values in sequence at each of these indices (as required by pinocchio) for continuous joints only. :param q: The original values where the values at indices `continuous_joint_q_ids` is filled with the actual value of joint position encoder. :type q: np.ndarray :param continuous_joint_q_ids: The indices in the q vector which are continuous joints. :type continuous_joint_q_ids: Sequence[float] :returns: Updated q vector with appropriate 2D representation for continuous joints. :rtype: np.ndarray .. py:method:: recover_generalised_pos_from_pinocchio_configuration(q: numpy.ndarray, continuous_joint_q_ids: Sequence[float] = None) -> numpy.ndarray :staticmethod: Static method to get the generalised coordinate position vector from pinocchio q vector. This function is mainly for handling situations where the robot has continuous joints. Pinocchio uses a 2D representation for a single continuous joint, and this function recovers single joint position values for such joints. :param q: Configuration vector in the format used by Pinocchio. :type q: np.ndarray :param continuous_joint_q_ids: The joint ids of continuous joints. Defaults to []. :type continuous_joint_q_ids: Sequence[float], optional :returns: Output generalised coordinate vector after removing pinocchio's continuous joint representation. :rtype: np.ndarray .. py:attribute:: floating_base .. py:attribute:: name Name of the robot model. .. py:attribute:: total_mass .. py:attribute:: num_joints .. py:attribute:: joint_names .. py:attribute:: joint_lower_limits :type: List[float] .. py:attribute:: joint_upper_limits :type: List[float] .. py:attribute:: joint_neutral_position .. py:attribute:: joint_velocity_limit :type: List[float] .. py:attribute:: actuated_joint_names :type: List[str] :value: [] .. py:attribute:: actuated_joint_q_ids :type: List[int] :value: [] index of joints in the generalised coordinate vector (q) .. py:attribute:: q The pinocchio q vector (generalised coordinates). .. py:attribute:: v The pinocchio v vector (generalised velocity). .. py:attribute:: a never filled! :type: The pinocchio a output vector (generalised acceleration). NOTE .. py:attribute:: actuated_joint_v_ids :type: List[int] :value: [] index of joints in the generalised velocity vector (v) .. py:attribute:: continuous_joint_q_ids :type: List[int] :value: [] Indices corresponding to continuous joints in the pinocchio configuration vector q. In pinocchio, continuous joints are represented using 2 values [cos(theta), sin(theta)]. This list contains the index to the first (cos) value (add 1 to the index to get the next). .. py:attribute:: continuous_joint_names :type: List[str] :value: [] .. py:attribute:: num_of_actuated_joints .. py:attribute:: additional_dims_ .. py:attribute:: expected_nq_ .. py:attribute:: actuated_joint_name_to_q_index :type: Mapping[str, int] Mapping from joint name to index of the joint configuration value in the pinocchio q vector. .. py:attribute:: actuated_joint_name_to_v_index :type: Mapping[str, int] Mapping from joint name to index of the joint velocity value in the pinocchio v vector. .. py:attribute:: actuated_joint_lower_limits .. py:attribute:: actuated_joint_upper_limits .. py:attribute:: actuated_joint_neutral_position .. py:attribute:: actuated_joint_limits mapping from {'joint_name' -> [lower_lim, upper_lim]} .. py:attribute:: num_of_frames .. py:attribute:: frame_names .. py:attribute:: frame_ids .. py:attribute:: ee_names Names of frames that are used as end-effectors. .. py:method:: print_model_info() .. py:method:: update(global_base_position: Vector3D, global_base_quaternion: QuatType, local_base_velocity_linear: Vector3D, local_base_velocity_angular: Vector3D, joint_positions: numpy.ndarray, joint_velocities: numpy.ndarray, joint_order: List[str] = None) Do all forward kinematics and dynamics computations for the given robot states. Also updates the internal configuration vectors (self.q, self.v). :param global_base_position: Position of robot base in the world. :type global_base_position: Vector3D :param global_base_quaternion: Orientation quaternion (x,y,z,w) of the robot base in the world. :type global_base_quaternion: QuatType :param local_base_velocity_linear: Linear velocity of the robot base in the local base frame. :type local_base_velocity_linear: Vector3D :param local_base_velocity_angular: Angular velocity of the robot base in the local base frame. :type local_base_velocity_angular: Vector3D :param joint_positions: Current joint positions of the robot. :type joint_positions: np.ndarray :param joint_velocities: Current joint velocities of the robot. :type joint_velocities: np.ndarray :param joint_order: Joint names in the order the position values were given. If None provided, uses default order found by pinocchio (urdf order). :type joint_order: List[str], optional .. py:method:: get_frame_id(frame_name: str) -> int .. py:method:: get_com_position() -> Vector3D .. py:method:: get_com_velocity() -> Vector3D .. py:method:: get_centroidal_mass() -> float .. py:method:: get_gravity_vector() -> numpy.ndarray .. py:method:: get_centroidal_inertia() -> numpy.ndarray .. py:method:: get_centroidal_momentum_map() -> numpy.ndarray .. py:method:: get_centroidal_momentum_map_linear() -> numpy.ndarray .. py:method:: get_centroidal_momentum_map_angular() -> numpy.ndarray .. py:method:: get_inertia_matrix() -> numpy.ndarray .. py:method:: get_nonlinear_effects() -> numpy.ndarray .. py:method:: get_joint_positions(joint_names: List[str] = None) -> numpy.ndarray Get the current joint positions using the latest joint configuration vector. This method also handles continuous joints and extracts joint position values for continuous joints from the 2D configuration representation used in pinocchio. :param joint_names: Optional list of joints/joint order. Defaults to None (uses default joint order). :type joint_names: List[str], optional :returns: Values of joint position/orientation for specified/all joints. :rtype: np.ndarray .. py:method:: get_joint_positions_from_configuration(q: numpy.ndarray, joint_names: List[str] = None) -> numpy.ndarray Get the joint positions from the provided pinocchio configuration vector. This method also handles continuous joints and extracts joint position values for continuous joints from the 2D configuration representation used in pinocchio. NOTE: This assumes the provided q vector has the same order as the self.q values in this class. :param q: Pinocchio configuration vector from which joint values are to be extracted. Assumes the provided q vector has the same order as the self.q values in this class. :type q: np.ndarray :param joint_names: Optional list of joints/joint order. Defaults to None (uses default joint order). :type joint_names: List[str], optional :returns: Values of joint position/orientation for specified/all joints. :rtype: np.ndarray .. py:method:: get_generalised_coordinates_from_q(q: numpy.ndarray) -> numpy.ndarray Get the generalised coordinate representation for the state of the robot. This is going to be ordered joint position values in case of fixed-base robots, and concatenation of base position, base quaternion (x,y,z,w) and joint positions for floating base systems. :param q: Input Pinocchio q vector. :type q: np.ndarray :returns: output generalised coordinate state representation of this robot. :rtype: np.ndarray .. py:method:: get_pinocchio_q_and_v_from_robot_state(global_base_position: Vector3D, global_base_quaternion: QuatType, local_base_velocity_linear: Vector3D, local_base_velocity_angular: Vector3D, joint_positions: numpy.ndarray, joint_velocities: numpy.ndarray, joint_order: List[str] = None) -> Tuple[numpy.ndarray, numpy.ndarray] Get pinocchio configuration vector (q) and velocity (v) from robot state data. Also handles continuous joints. :param global_base_position: Position of robot base in the world. :type global_base_position: Vector3D :param global_base_quaternion: Orientation quaternion (x,y,z,w) of the robot base in the world. :type global_base_quaternion: QuatType :param local_base_velocity_linear: Linear velocity of the robot base in the local base frame. :type local_base_velocity_linear: Vector3D :param local_base_velocity_angular: Angular velocity of the robot base in the local base frame. :type local_base_velocity_angular: Vector3D :param joint_positions: Current joint positions of the robot. :type joint_positions: np.ndarray :param joint_velocities: Current joint velocities of the robot. :type joint_velocities: np.ndarray :param joint_order: Joint names in the order the position values were given. If None provided, uses default order found by pinocchio (urdf order). :type joint_order: List[str], optional :returns: The q and v vectors that can be used for pinocchio operations. :rtype: Tuple[np.ndarray, np.ndarray] .. py:method:: get_frame_position(frame_name: str, reference_frame: str = 'world') -> Vector3D .. py:method:: get_global_frame_pose(frame_name: str) -> Tuple[Vector3D, QuatType] .. py:method:: get_ee_poses(ee_names: List[str] = None) -> List[Tuple[Vector3D, QuatType]] .. py:method:: get_ee_velocities(ee_names: List[str] = None) -> List[Tuple[Vector3D, Vector3D]] .. py:method:: get_frame_rotation(frame_name: str, reference_frame: str = 'world') -> numpy.ndarray .. py:method:: get_frame_quaternion(frame_name: str, reference_frame: str = 'world') -> QuatType .. py:method:: get_frame_transformation(frame_name: str, reference_frame: str = 'world') -> numpy.ndarray .. py:method:: get_frame_velocity_linear(frame_name: str, reference_frame: str = 'world') -> Vector3D .. py:method:: get_frame_velocity_angular(frame_name: str, reference_frame: str = 'world') -> Vector3D .. py:method:: get_frame_acceleration_linear(frame_name: str, reference_frame: str = 'world') -> Vector3D .. py:method:: get_frame_acceleration_angular(frame_name: str, reference_frame: str = 'world') -> Vector3D .. py:method:: get_frame_jacobian(frame_name: str, reference_frame: str = 'world') -> numpy.ndarray .. py:method:: get_frame_jacobian_linear(frame_name: str, reference_frame: str = 'world') -> numpy.ndarray .. py:method:: get_frame_jacobian_angular(frame_name: str, reference_frame: str = 'world') -> numpy.ndarray