Drake中IIWA机械臂差分IK初始速度过大问题咨询

问题描述 #

我是Drake框架及机械臂运动学领域的新手，已对iiwa_teleop代码进行修改，将遥操作模块替换为可从命令行（CLI）接收目标位姿的小型系统。我已设置关节速度限制，但在仿真过程中，机械臂初始阶段移动速度极快，随后逐渐减速，无法定位问题原因，特此咨询。

完整代码 #

import argparse
import numpy as np

from manipulation.meshcat_utils import WsgButton
from manipulation.scenarios import AddIiwaDifferentialIK
from manipulation.station import LoadScenario
from pydrake.all import (
    ApplySimulatorConfig,
    DiagramBuilder,
    MeshcatPoseSliders,
    MeshcatVisualizer,
    Simulator,
    AbstractValue,
    ConstantValueSource,
)
from pydrake.systems.framework import LeafSystem, BasicVector
from iiwa_setup.iiwa import IiwaForwardKinematics, IiwaHardwareStationDiagram
from pydrake.math import RigidTransform, RollPitchYaw
from pydrake.common.value import Value
from pydrake.multibody.inverse_kinematics import (
    DifferentialInverseKinematicsIntegrator,
    DifferentialInverseKinematicsParameters,
)


class CLIPoseCommand(LeafSystem):
    def __init__(self):
        super().__init__()
        self.DeclareAbstractOutputPort(
            "X_AE_desired", lambda: Value(RigidTransform()), self.CalcPoseOutput
        )
        self.DeclareAbstractInputPort("ee_pose", Value(RigidTransform()))
        self.pose_rpy_xyz = np.zeros(6)

    def CalcPoseOutput(self, context, output):
        rpy = RollPitchYaw(self.pose_rpy_xyz[:3])
        xyz = self.pose_rpy_xyz[3:]
        output.get_mutable_value().set(RollPitchYaw(rpy).ToRotationMatrix(), xyz)

    def update_pose(self, X_WE: RigidTransform):
        rpy = RollPitchYaw(X_WE.rotation()).vector()
        xyz = X_WE.translation()
        self.pose_rpy_xyz = np.hstack((rpy, xyz))


def main(use_hardware: bool, has_wsg: bool) -> None:
    scenario_data = """
    directives:
    - add_directives:
        file: package://iiwa_setup/iiwa7.dmd.yaml
    plant_config:
        # For some reason, this requires a small timestep
        time_step: 0.005
        contact_model: "hydroelastic"
        discrete_contact_approximation: "sap"
    model_drivers:
        iiwa: !IiwaDriver
            lcm_bus: "default"
            control_mode: position_only
    lcm_buses:
        default:
            lcm_url: ""
    """

    builder = DiagramBuilder()

    scenario = LoadScenario(data=scenario_data)
    station: IiwaHardwareStationDiagram = builder.AddNamedSystem(
        "station",
        IiwaHardwareStationDiagram(
            scenario=scenario, has_wsg=has_wsg, use_hardware=use_hardware
        ),
    )
    controller_plant = station.get_iiwa_controller_plant()

    lower_bnd = np.array([-0.1, -0.1, -0.1, -0.1, -0.1, -0.1, -0.1]) / 2
    upper_bnd = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) / 2

    params = DifferentialInverseKinematicsParameters(
        controller_plant.num_positions(), controller_plant.num_velocities()
    )

    time_step = 0.005
    params.set_time_step(time_step)
    params.set_joint_velocity_limits((lower_bnd, upper_bnd))

    differential_ik = builder.AddSystem(
        DifferentialInverseKinematicsIntegrator(
            controller_plant,
            controller_plant.GetFrameByName("iiwa_link_7"),
            time_step,
            params,
        )
    )

    builder.Connect(
        differential_ik.get_output_port(),
        station.GetInputPort("iiwa.position"),
    )
    builder.Connect(
        station.GetOutputPort("iiwa.state_estimated"),
        differential_ik.GetInputPort("robot_state"),
    )

    cli_cmder = CLIPoseCommand()
    pose_command_system = builder.AddSystem(cli_cmder)
    builder.Connect(
        cli_cmder.get_output_port(), differential_ik.GetInputPort("X_AE_desired")
    )

    iiwa_forward_kinematics = builder.AddSystem(
        IiwaForwardKinematics(station.get_internal_plant())
    )
    builder.Connect(
        station.GetOutputPort("iiwa.position_commanded"),
        iiwa_forward_kinematics.get_input_port(),
    )
    builder.Connect(
        iiwa_forward_kinematics.get_output_port(), cli_cmder.get_input_port()
    )

    # Required for visualizing the internal station
    _ = MeshcatVisualizer.AddToBuilder(
        builder, station.GetOutputPort("query_object"), station.internal_meshcat
    )

    diagram = builder.Build()
    simulator = Simulator(diagram)
    ApplySimulatorConfig(scenario.simulator_config, simulator)
    simulator.set_target_realtime_rate(1.0)

    station.internal_meshcat.AddButton("Stop Simulation")
    simulator.AdvanceTo(0.1)

    user_input = input("Enter roll pitch yaw x y z").split()
    if len(user_input) == 6:
        roll_deg, pitch_deg, yaw_deg, x, y, z = list(map(float, user_input))
        X_goal = RigidTransform(
            RollPitchYaw(
                np.radians(roll_deg), np.radians(pitch_deg), np.radians(yaw_deg)
            ),
            [x, y, z],
        )
        cli_cmder.update_pose(X_goal)

    while station.internal_meshcat.GetButtonClicks("Stop Simulation") < 1:
        simulator.AdvanceTo(simulator.get_context().get_time() + 1)
        station.internal_meshcat.DeleteButton("Stop Simulation")


if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument(
        "--use_hardware",
        action="store_true",
        help="Whether to use real world hardware.",
    )
    parser.add_argument(
        "--has_wsg",
        action="store_true",
        help="Whether the iiwa has a WSG gripper or not.",
    )
    args = parser.parse_args()
    main(use_hardware=args.use_hardware, has_wsg=args.has_wsg)

问题排查与解决建议 #

1. 初始位姿突变导致的冲击 #

你在仿真启动后直接通过update_pose设置目标位姿，机械臂从初始姿态到目标姿态的差值过大，微分IK会尝试以最大允许速度追赶目标，造成初始阶段速度暴增。解决方法是添加平滑轨迹过渡，让机械臂在指定时间内从当前位姿逐步移动到目标位姿：

# 在获取用户输入后，替换cli_cmder.update_pose(X_goal)的代码
from pydrake.trajectories import PiecewisePose

# 获取当前末端执行器位姿
diagram_context = diagram.GetMutableContextFromRoot(simulator.get_context())
current_pose = iiwa_forward_kinematics.get_output_port().Eval(diagram_context)

# 生成2秒内从当前位姿到目标位姿的线性平滑轨迹
traj = PiecewisePose.MakeLinear([0, 2], [current_pose, X_goal])

# 创建轨迹输出系统
class TrajectorySource(LeafSystem):
    def __init__(self, traj):
        super().__init__()
        self.traj = traj
        self.DeclareAbstractOutputPort(
            "X_AE_desired", lambda: Value(RigidTransform()), self.CalcOutput
        )
    
    def CalcOutput(self, context, output):
        t = context.get_time() - simulator.get_context().get_time()
        output.get_mutable_value().set(self.traj.GetPose(t))

# 添加系统并重新连接
traj_source = builder.AddSystem(TrajectorySource(traj))
builder.Disconnect(cli_cmder.get_output_port(), differential_ik.GetInputPort("X_AE_desired"))
builder.Connect(traj_source.get_output_port(), differential_ik.GetInputPort("X_AE_desired"))

2. 补充任务空间速度限制 #

仅设置关节速度限制不足以约束末端执行器的运动速度，微分IK会优先满足任务空间的位姿跟踪需求。可以添加末端执行器的线速度和角速度限制：

# 在设置params.set_joint_velocity_limits后添加
# 末端线速度限制（m/s）：x、y、z方向
linear_vel_limits = [0.1, 0.1, 0.1]
# 末端角速度限制（rad/s）：roll、pitch、yaw方向
angular_vel_limits = [0.2, 0.2, 0.2]
params.set_end_effector_velocity_limits(linear_vel_limits, angular_vel_limits)

3. 初始化微分IK的积分状态 #

确保微分IK的初始积分位置与机械臂的初始关节位置一致，避免积分器初始偏差导致的突跳：

# 在diagram = builder.Build()之后添加
diff_ik_context = differential_ik.GetMyContextFromRoot(simulator.get_context())
initial_joint_state = station.GetOutputPort("iiwa.state_estimated").Eval(simulator.get_context())
differential_ik.SetPositions(diff_ik_context, initial_joint_state[:7])
differential_ik.SetVelocities(diff_ik_context, initial_joint_state[7:])

4. 优化仿真循环步进 #

当前循环每次推进1秒，会导致视觉上的“瞬间移动”。改为持续推进仿真直到停止按钮被点击：

# 替换原来的while循环
while station.internal_meshcat.GetButtonClicks("Stop Simulation") < 1:
    simulator.AdvanceTo(simulator.get_context().get_time() + 0.01)

内容的提问来源于stack exchange，提问作者Liu Jinkua