Package: dm-robotics-controllers
DM Robotics: Controllers Library (Python)Contents:Cartesian 6D to Joint Velocity MapperCartesian 6D to Joint Velocity MapperPython bindings fordm_robotics/controllers/lsqp/cartesian_6d_to_joint_velocity_mapper.This module consists of two classes:cartesian_6d_to_joint_velocity_mapper.Parameterscartesian_6d_to_joint_velocity_mapper.MapperThe mapper solves a constrained linear least-squares optimization problem at
every iteration to compute the joint velocities that best achieve the desired
Cartesian 6D velocity of an object.In its most basic configuration, it computes the joint velocities that achieve
the desired Cartesian 6d velocity with singularity robustness. In addition, this
mapper also supports the following functionality:Nullspace control can be enabled to bias the joint velocities to a desired
value without affecting the accuracy of the resultant Cartesian velocity;Collision avoidance can be enabled between any two MuJoCo geoms;Limits on the joint positions, velocities, and accelerations can be defined
to ensure that the computed joint velocities do not result in limit
violations.Please refer todm_robotics/controllers/lsqp/cartesian_6d_to_joint_velocity_mapper.hor the
class docstrings for more information.Dependencies:dm_robotics/least_squares_qpdm_robotics/controllersdm_controlUsagefromdm_controlimportmujocofromdm_control.mujoco.wrapper.mjbindingsimportenumsfromdm_robotics.controllersimportcartesian_6d_to_joint_velocity_mapper# Initialize simulation. Assumes velocity controlled robot.# physics.data.ctrl[:] is an array of size 7 that corresponds to the commanded# velocities of the joints with IDs 7, 8, 9, 10, 12, 13, 14.physics=mujoco.Physics(...)# Create MuJoCo physics.# Create mapper parameters.params=cartesian_6d_to_joint_velocity_mapper.Parameters()## Set model parameters.params.model=physics.modelparams.joint_ids=[7,8,9,10,12,13,14]# MuJoCo joint IDs being controlled.params.object_type=enums.mjtObj.mjOBJ_SITE# MuJoCo object being controlled.params.object_name="end_effector"# name of MuJoCo object being controlled.params.integration_timestep=0.005# Amount of time the joint velocities will be executed for.## Enable joint position limit constraint. Limits are read automatically from the# model.params.enable_joint_position_limits=Trueparams.joint_position_limit_velocity_scale=0.95params.minimum_distance_from_joint_position_limit=0.01# ~0.5deg.## Enable joint velocity limits.params.enable_joint_velocity_limits=Trueparams.joint_velocity_magnitude_limits=[0.5,0.5,0.5,0.5,0.5,0.5,0.5]## Enable joint acceleration limits.params.enable_joint_acceleration_limits=Trueparams.remove_joint_acceleration_limits_if_in_conflict=Trueparams.joint_acceleration_magnitude_limits=[10.0,10.0,10.0,10.0,10.0,10.0,10.0]## Enable collision avoidance between the following geoms:#   * "gripper" and "base_link"#   * "base_link" and "floor"#   * "link1" and "floor"#   * "gripper" and "floor"#   * "link1" and "link4"#   * "link1" and "link5"#   * "link1" and "link6"#   * "link2" and "link4"#   * "link2" and "link5"#   * "link2" and "link6"# Note that collision avoidance will not be enabled for a pair of geoms if they# are attached to the same body or are attached to bodies that have a# parent-child relationship.params.enable_collision_avoidance=Trueparams.collision_avoidance_normal_velocity_scale=0.5params.minimum_distance_from_collisions=0.01params.collision_detection_distance=0.3params.collision_pairs=[(["gripper"],["base_link"]),(["base_link","link1","gripper"],["floor"]),(["link1","link2"],["link4","link5","link6"])]## Numerical stability parameters.params.check_solution_validity=Trueparams.solution_tolerance=1e-3params.regularization_weight=1e-2params.enable_nullspace_control=Trueparams.return_error_on_nullspace_failure=Falseparams.nullspace_projection_slack=1e-7# Create mapper.mapper=cartesian_6d_to_joint_velocity_mapper.Mapper(params)# Compute joint velocities and apply them to the joint velocity actuator# commands at every step.whileTrue:# The nullspace bias is often chosen to be a velocity towards the mid-range of# the joints, but can be chosen to be any 7D joint velocity vector.nullspace_joint_velocity_bias=get_nullspace_bias()target_cartesian_velocity=get_end_effector_target_velocity()solution=mapper.compute_joint_velocities(physics.data,target_velocity,nullspace_bias)physics.data.ctrl[:]=solutionphysics.step()