0

我正在尝试使用 Python 构建 MultibodyPlant，而不是从 URDF 加载它。即，将 CPP 代码make_cartpole_plant.cc翻译成 Python。

我找到了所有 Python 绑定，除了AddJointActuator. 我在python doc中找不到这个函数声明。也许这个函数没有 Python 绑定？我想知道是否有办法从 Python 添加执行器。

这是我的代码：它有效。AddJointActuator但我不知道如何将执行器添加到我的购物车，因为我在 CPP 中找不到该函数的 Python 绑定。

arm_length = 1.
arm_mass = 1
arm_name = "arm"
arm_radius = 0.1

shoulder_name = "shoulder"

unit_y =  np.array([0., 1., 0.])
unit_z =  np.array([0., 0., 1.])
g = -9.8

def main():
    builder = DiagramBuilder()
    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)

    # Bo=body frame's origin
    # Bcm=body's center of mass
    # B = in body frame B
    # S = body

    # COM's positions in each link L frame:
    # Frame L's origin is located at the shoulder outboard frame.
    p_L1L1cm = - arm_length * unit_z

    # Define each link's spatial inertia about their respective COM.
    G1_Bcm = straight_line_unit_inertia(arm_length, unit_z)
    M1_L1o = SpatialInertia(mass=arm_mass, p_PScm_E=p_L1L1cm, G_SP_E=G1_Bcm)
    arm_link = plant.AddRigidBody(name=arm_name, M_BBo_B=M1_L1o)

    assert plant.geometry_source_is_registered()
    # Pose of the geometry for link in the link's frame.
    X_L1G1 = RigidTransform(p=-arm_length/2.*unit_z)
    cylindar = Cylinder(radius=arm_radius, length=arm_length)
    arm_vis = plant.RegisterVisualGeometry(body=arm_link, X_BG=X_L1G1, shape=cylindar, name="visual", diffuse_color=[0,1,0,1])

    # Register some (anchored) geometry to the world.
    # Default is identity transform.
    X_WG = RigidTransform()
    plant.RegisterVisualGeometry(body=plant.world_body(), X_BG=X_WG, shape=Sphere(arm_length/8.0), name="visual", diffuse_color=[1,0,0,1])

    # Shoulder inboard frame Si IS the the world frame W.
    # Shoulder outboard frame So IS frame L1.
    # The link rotates in the x-z plane.
    shoulder_joint = RevoluteJoint(name=shoulder_name, frame_on_parent=plant.world_frame(), frame_on_child=arm_link.body_frame(), axis=unit_y, damping=0.0)
    plant.AddJoint(shoulder_joint)

    # Add acrobot's actuator at the elbow joint.
    # plant->AddJointActuator(params.actuator_name(), elbow);

    # Gravity acting in the -z direction.
    plant.mutable_gravity_field().set_gravity_vector(g * unit_z)

    # 3. Add Drake Visualizer.
    ConnectDrakeVisualizer(builder, scene_graph)
    plant.Finalize()
    diagram = builder.Build()
    simulator = Simulator(diagram)
    context = simulator.get_mutable_context()
    # x, x_dot
    context.SetContinuousState([0.5*np.pi, 0.])
    # context.FixInputPort(0, [0., 0.])
    simulator.Initialize()
    time = 2.
    target_rate = simulator.get_target_realtime_rate()
    simulator.set_target_realtime_rate(1.)
    simulator.AdvanceTo(time)
    simulator.set_target_realtime_rate(target_rate)
4

1 回答 1

0

你是对的。缺少该方法的绑定，但添加起来很简单： https ://github.com/RobotLocomotion/drake/pull/13092

它应该很快在 master 中可用。

于 2020-04-20T01:45:53.023 回答