Доступен ли AddJointActuator в Drake: Python? - PullRequest
Новая
       12

Доступен ли AddJointActuator в Drake: Python?

0 голосов
/ 20 апреля 2020

Я пытаюсь создать MultibodyPlant, используя Python, а не загружать его из URDF. т.е. перевод CPP кода make_cartpole_plant. cc в Python.

Я нашел все Python привязки, кроме AddJointActuator. Я не смог найти это объявление функции в python do c. Может быть, эта функция не имеет привязки Python? Мне интересно, есть ли способ добавить привод из Python.

Это мой код: он работает. Но я не знаю, как добавить привод к моей тележке, потому что не могу найти привязку Python для функции AddJointActuator в CPP. 

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)

1 Ответ

0 голосов
/ 20 апреля 2020

Вы правы. Привязка для этого метода отсутствовала, но ее было просто добавить: https://github.com/RobotLocomotion/drake/pull/13092

Она должна скоро появиться в master.

...