def test_geometry_with_params(self):
builder = DiagramBuilder()
plant = builder.AddSystem(RimlessWheel())
scene_graph = builder.AddSystem(SceneGraph())
geom = RimlessWheelGeometry.AddToBuilder(
builder=builder,
floating_base_state_port=plant.get_floating_base_state_output_port(), # noqa
rimless_wheel_params=RimlessWheelParams(), scene_graph=scene_graph)
# Confirming that the resulting diagram builds.
builder.Build()
self.assertIsInstance(geom, RimlessWheelGeometry)
def test_params(self):
params = RimlessWheelParams()
params.set_mass(1.)
params.set_length(2.)
params.set_gravity(4.)
params.set_number_of_spokes(7)
params.set_slope(.15)
self.assertEqual(params.mass(), 1.)
self.assertEqual(params.length(), 2.)
self.assertEqual(params.gravity(), 4.)
self.assertEqual(params.number_of_spokes(), 7)
self.assertEqual(params.slope(), .15)
"--initial_angular_velocity",
type=float,
help="Initial angular velocity of the stance leg "
"(in radians/sec).",
default=5.0)
parser.add_argument("-S",
"--slope",
type=float,
help="Ramp angle (in radians)",
default=0.08)
args = parser.parse_args()
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/rimless_wheel/RimlessWheel.urdf"),
FloatingBaseType.kRollPitchYaw)
params = RimlessWheelParams()
params.set_slope(args.slope)
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()
rimless_wheel = builder.AddSystem(RimlessWheel())
visualizer = builder.AddSystem(
import argparse
import math
import numpy as np
from pydrake.all import (Box, DiagramBuilder, FindResourceOrThrow,
FloatingBaseType, Isometry3, RigidBodyTree, Simulator,
VisualElement)
from pydrake.examples.rimless_wheel import (RimlessWheel, RimlessWheelParams)
from underactuated import (PlanarRigidBodyVisualizer)
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/rimless_wheel/RimlessWheel.urdf"),
FloatingBaseType.kRollPitchYaw)
params = RimlessWheelParams()
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()
rimless_wheel = builder.AddSystem(RimlessWheel())
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
import numpy as np
import matplotlib.pyplot as plt
from pydrake.examples.pendulum import PendulumPlant, PendulumParams
from pydrake.examples.rimless_wheel import RimlessWheelParams
from pydrake.all import DirectCollocation, Solve
# This is just a placeholder... until we have more support for hybrid
# trajectory optimization, we'll do optimization on an equivalent pendulum.
plant = PendulumPlant()
context = plant.CreateDefaultContext()
rw_params = RimlessWheelParams()
alpha = np.pi / rw_params.number_of_spokes()
pendulum_params = context.get_numeric_parameter(0)
pendulum_params.set_mass(rw_params.mass())
pendulum_params.set_length(rw_params.length())
pendulum_params.set_damping(0.)
dircol = DirectCollocation(plant,
context,
num_time_samples=11,
minimum_timestep=0.01,
maximum_timestep=0.1)
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddConstraintToAllKnotPoints(
dircol.state()[0] >= np.pi + rw_params.slope() - alpha)
dircol.AddConstraintToAllKnotPoints(
dircol.state()[0] <= np.pi + rw_params.slope() + alpha)
help="Initial angle of the stance leg (in radians).",
default=0.0)
parser.add_argument("-V", "--initial_angular_velocity",
type=float,
help="Initial angular velocity of the stance leg "
"(in radians/sec).",
default=5.0)
parser.add_argument("-S", "--slope", type=float,
help="Ramp angle (in radians)",
default=0.08)
args = parser.parse_args()
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/rimless_wheel/RimlessWheel.urdf"),
FloatingBaseType.kRollPitchYaw)
params = RimlessWheelParams()
params.set_slope(args.slope)
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()
rimless_wheel = builder.AddSystem(RimlessWheel())
visualizer = builder.AddSystem(PlanarRigidBodyVisualizer(tree,
def test_params(self):
params = RimlessWheelParams()
params.set_mass(1.)
params.set_length(2.)
params.set_gravity(4.)
params.set_number_of_spokes(7)
params.set_slope(.15)
self.assertEqual(params.mass(), 1.)
self.assertEqual(params.length(), 2.)
self.assertEqual(params.gravity(), 4.)
self.assertEqual(params.number_of_spokes(), 7)
self.assertEqual(params.slope(), .15)
help="Initial angle of the stance leg (in radians).",
default=0.0)
parser.add_argument("-V",
"--initial_angular_velocity",
type=float,
help="Initial angular velocity of the stance leg "
"(in radians/sec).",
default=5.0)
parser.add_argument("-S",
"--slope",
type=float,
help="Ramp angle (in radians)",
default=0.08)
args = parser.parse_args()
params = RimlessWheelParams()
params.set_slope(args.slope)
builder = DiagramBuilder()
rimless_wheel = builder.AddSystem(RimlessWheel())
scene_graph = builder.AddSystem(SceneGraph())
RimlessWheelGeometry.AddToBuilder(
builder, rimless_wheel.get_floating_base_state_output_port(), params,
scene_graph)
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-8., 8.], ylim=[-2., 3.]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)