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) self.assertEqual(RimlessWheel.calc_alpha(params=params), np.pi / params.number_of_spokes())
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)