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)
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)
"(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,
xlim=[-8., 8.],
ylim=[-2., 3.],
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)
dircol.AddConstraint(dircol.initial_state()[0] == np.pi + rw_params.slope() -
alpha)
dircol.AddConstraint(dircol.final_state()[0] == np.pi + rw_params.slope() +
alpha)
dircol.AddConstraint(dircol.initial_state()[1] == dircol.final_state()[1] *
np.cos(2 * alpha))
result = Solve(dircol)
assert (result.is_success())
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,
xlim=[-8., 8.],
ylim=[-2., 3.],
figsize_multiplier=3))