DM Robotics：控制器库（Python）

内容

• 笛卡尔6D到关节速度映射器

笛卡尔6D到关节速度映射器

为dm_robotics/controllers/lsqp/cartesian_6d_to_joint_velocity_mapper提供Python绑定。

此模块包含两个类

• cartesian_6d_to_joint_velocity_mapper.Parameters
• cartesian_6d_to_joint_velocity_mapper.Mapper

映射器在每个迭代中解决一个约束线性最小二乘优化问题，以计算能够最好地实现对象所需笛卡尔6D速度的关节速度。

在其最基本的配置中，它计算能够实现所需笛卡尔6d速度且具有奇点鲁棒性的关节速度。此外，此映射器还支持以下功能

• 可以通过启用零空间控制，在不影响结果笛卡尔速度准确性的情况下，将关节速度偏置到期望值；
• 可以在任何两个 MuJoCo 几何体之间启用碰撞避免；
• 可以定义关节位置、速度和加速度的限制，以确保计算的关节速度不会导致越限；

请参阅 dm_robotics/controllers/lsqp/cartesian_6d_to_joint_velocity_mapper.h 或类文档字符串以获取更多信息。

依赖关系

• dm_robotics/least_squares_qp
• dm_robotics/controllers
• dm_control

用法

from dm_control import mujoco
from dm_control.mujoco.wrapper.mjbindings import enums
from dm_robotics.controllers import cartesian_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.model
params.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 = True
params.joint_position_limit_velocity_scale = 0.95
params.minimum_distance_from_joint_position_limit = 0.01  # ~0.5deg.
#
# Enable joint velocity limits.
params.enable_joint_velocity_limits = True
params.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 = True
params.remove_joint_acceleration_limits_if_in_conflict = True
params.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 = True
params.collision_avoidance_normal_velocity_scale = 0.5
params.minimum_distance_from_collisions = 0.01
params.collision_detection_distance = 0.3
params.collision_pairs = [(["gripper"], ["base_link"]),
                          (["base_link", "link1", "gripper"], ["floor"]),
                          (["link1", "link2"], ["link4", "link5", "link6"])]
#
# Numerical stability parameters.
params.check_solution_validity = True
params.solution_tolerance = 1e-3
params.regularization_weight = 1e-2
params.enable_nullspace_control = True
params.return_error_on_nullspace_failure = False
params.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.
while True:
  # 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[:] = solution
  physics.step()