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,
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)