Pydrake：为 RigidTransformations 创建轨迹源

Question

我一直在尝试为 RigidTransforms 创建一个 TrajectorySource 以传递到 DifferentialInverseKinematicsIntegrator，它只在其输入端口接收 RigidTransforms。

def createTraj(time, pose):
    times = []
    poses = []
    for step in time:
        times.append(time[step])
        poses.append(pose[step])
        
    return PiecewisePose.MakeLinear(times, poses)

最初，我试图将上面 createTraj 的输出直接传递给 TrajectorySource 但运行进入我的轨迹有多个列的问题： Failure at systems/primitives/trajectory_source.cc:21 in TrajectorySource(): condition 'trajectory.cols() == 1' failed.

import matplotlib.pyplot as plt, mpld3
class DexterTest():
# Output from createTraj is passed as parameter: traj into constructor
    def __init__(self, traj):
        builder = DiagramBuilder()
        self.station = DexterPPStation(1e-4, "/opt/drake/share/drake/manipulation/models/final_dexter_description/urdf/dexter.urdf")

        self.station.CreateBins("/opt/drake/share/drake/examples/manipulation_station/models/bin.sdf", RigidTransform(np.array([0.5,0,0])), RigidTransform(np.array([0,0.5,0])))
        self.station.CreateRandomPickingObjects(3)
        self.station.AddDexter()
        builder.AddSystem(self.station)
        self.station.Finalize()
        
        self.diff_ik = DifferentialInverseKinematicsIntegrator(self.station.controller_plant, self.station.plant.GetFrameByName("link6", self.station.dexter["instance"]), self.station.time_step, DifferentialInverseKinematicsParameters(7,7))
        builder.AddSystem(self.diff_ik)
#=========================================== Likely Source of Error ===========================================
        pose = builder.AddSystem(PoseSystem())
        p_G_source = builder.AddSystem(TrajectorySource(traj.get_position_trajectory()))
        w_G_source = builder.AddSystem(TrajectorySource(traj.get_orientation_trajectory()))

        builder.Connect(p_G_source.get_output_port(), pose.GetInputPort("p_G"))
        builder.Connect(w_G_source.get_output_port(), pose.GetInputPort("r_G"))

        builder.Connect(pose.get_output_port(), self.diff_ik.get_input_port())
#======================================================================================           
        MeshcatVisualizerCpp.AddToBuilder(builder, self.station.GetOutputPort("query_object"), meshcat)
        self.diagram = builder.Build()
        self.simulator = Simulator(self.diagram)
        self.diagram_context = self.simulator.get_mutable_context()
        self.station_context = self.station.GetMyMutableContextFromRoot(self.diagram_context)
        self.plant_context = self.station.GetSubsystemContext(self.station.plant, self.station_context)
        self.station.SetRandomPoses(self.plant_context)
        builder.Connect(self.diff_ik.get_output_port(), self.station.GetInputPort("dexter_position"))
        
    def run(self):
        self.simulator.set_target_realtime_rate(2.0)
        self.simulator.AdvanceTo(1)
        
class PoseSystem(LeafSystem):
    def __init__(self):
        LeafSystem.__init__(self)
        
        self.p_G = self.DeclareVectorInputPort("p_G", BasicVector(3))
        self.r_G = self.DeclareVectorInputPort("r_G", BasicVector(4))
        self.DeclareAbstractOutputPort("X_G", Value[RigidTransform], self.CalcOutput)
        
        
    def CalcOutput(self, context, output):
        pose = RigidTransform(Quaternion(self.r_G.Eval(context)), self.p_G.Eval(context))
        output.set_value(pose)

相反，我尝试将我的轨迹分解成它的方向和位置部分，将它们添加到自定义系统的输入端口，然后在输出端口中一起重建它们。但是，一旦调用 run 方法，这就会给我以下 RuntimeError：RuntimeError: This multibody element does not belong to the supplied MultibodyTree.

如有任何帮助，我们将不胜感激！

Answer 1

我觉得你很亲近。 PoseSystem 看起来应该是您在 post 中阐明的问题的解决方案。（关于 MultibodyTree 的错误必须来自您代码的其他部分。

您实际上不需要将 RigidTransform 分解为方向/平移来创建您的 PoseSystem，如果 poses 是一个 PiecewisePose 轨迹。

我在本笔记本的 PickAndPlaceTrajectory class 中有一个这样做的例子：https://github.com/RussTedrake/manipulation/blob/008cec6343dd39063705287e6664a3fee71a43b8/pose.ipynb

Pydrake：为 RigidTransformations 创建轨迹源

Pydrake: Creating a Trajectory Source for RigidTransformations

python

drake