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