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