def test_geometry_with_params(self):
builder = DiagramBuilder()
plant = builder.AddSystem(CompassGait())
scene_graph = builder.AddSystem(SceneGraph())
geom = CompassGaitGeometry.AddToBuilder(
builder=builder,
floating_base_state_port=plant.get_floating_base_state_output_port(
), # noqa
compass_gait_params=CompassGaitParams(),
scene_graph=scene_graph)
# Confirming that the resulting diagram builds.
builder.Build()
self.assertIsInstance(geom, CompassGaitGeometry)
import argparse
import math
import numpy as np
from pydrake.all import (Box, DiagramBuilder, FindResourceOrThrow,
FloatingBaseType, Isometry3, RigidBodyTree,
SignalLogger, Simulator, VisualElement)
from pydrake.examples.compass_gait import (CompassGait, CompassGaitParams)
from underactuated import (PlanarRigidBodyVisualizer)
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/compass_gait/CompassGait.urdf"),
FloatingBaseType.kRollPitchYaw)
params = CompassGaitParams()
R = np.identity(3)
R[0, 0] = math.cos(params.slope())
R[0, 2] = math.sin(params.slope())
R[2, 0] = -math.sin(params.slope())
R[2, 2] = math.cos(params.slope())
X = Isometry3(rotation=R, translation=[0, 0, -5.])
color = np.array([0.9297, 0.7930, 0.6758, 1])
tree.world().AddVisualElement(VisualElement(Box([100., 1., 10.]), X, color))
tree.compile()
builder = DiagramBuilder()
compass_gait = builder.AddSystem(CompassGait())
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
def test_params(self):
params = CompassGaitParams()
params.set_mass_hip(1.)
params.set_mass_leg(2.)
params.set_length_leg(3.)
params.set_center_of_mass_leg(4.)
params.set_gravity(5.)
params.set_slope(.15)
self.assertEqual(params.mass_hip(), 1.)
self.assertEqual(params.mass_leg(), 2.)
self.assertEqual(params.length_leg(), 3.)
self.assertEqual(params.center_of_mass_leg(), 4.)
self.assertEqual(params.gravity(), 5.)
self.assertEqual(params.slope(), .15)
def test_params(self):
params = CompassGaitParams()
params.set_mass_hip(1.)
params.set_mass_leg(2.)
params.set_length_leg(3.)
params.set_center_of_mass_leg(4.)
params.set_gravity(5.)
params.set_slope(.15)
self.assertEqual(params.mass_hip(), 1.)
self.assertEqual(params.mass_leg(), 2.)
self.assertEqual(params.length_leg(), 3.)
self.assertEqual(params.center_of_mass_leg(), 4.)
self.assertEqual(params.gravity(), 5.)
self.assertEqual(params.slope(), .15)
DiagramBuilder,
FindResourceOrThrow,
FloatingBaseType,
Isometry3,
RigidBodyTree,
SignalLogger,
Simulator,
VisualElement)
from pydrake.examples.compass_gait import (CompassGait, CompassGaitParams)
from underactuated import (PlanarRigidBodyVisualizer)
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/compass_gait/CompassGait.urdf"),
FloatingBaseType.kRollPitchYaw)
params = CompassGaitParams()
R = np.identity(3)
R[0, 0] = math.cos(params.slope())
R[0, 2] = math.sin(params.slope())
R[2, 0] = -math.sin(params.slope())
R[2, 2] = math.cos(params.slope())
X = Isometry3(rotation=R, translation=[0, 0, -5.])
color = np.array([0.9297, 0.7930, 0.6758, 1])
tree.world().AddVisualElement(VisualElement(Box([100., 1., 10.]), X, color))
tree.compile()
builder = DiagramBuilder()
compass_gait = builder.AddSystem(CompassGait())
hip_torque = builder.AddSystem(ConstantVectorSource([0.0]))
builder.Connect(hip_torque.get_output_port(0), compass_gait.get_input_port(0))