def test_pose_bundle(self): num_poses = 7 bundle = PoseBundle(num_poses) # - Accessors. self.assertEqual(bundle.get_num_poses(), num_poses) with catch_drake_warnings(expected_count=1): self.assertTrue(isinstance(bundle.get_pose(0), Isometry3)) self.assertTrue(isinstance(bundle.get_transform(0), RigidTransform)) self.assertTrue(isinstance(bundle.get_velocity(0), FrameVelocity)) # - Mutators. kIndex = 5 p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) with catch_drake_warnings(expected_count=2): bundle.set_pose(kIndex, Isometry3(q, p)) self.assertTrue((bundle.get_pose(kIndex).matrix() == Isometry3(q, p).matrix()).all()) pose = RigidTransform(quaternion=q, p=p) bundle.set_transform(kIndex, pose) self.assertTrue((bundle.get_transform(kIndex).GetAsMatrix34() == pose.GetAsMatrix34()).all()) w = [0.1, 0.3, 0.5] v = [0., 1., 2.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) bundle.set_velocity(kIndex, frame_velocity) vel_actual = bundle.get_velocity(kIndex).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) name = "Alice" bundle.set_name(kIndex, name) self.assertEqual(bundle.get_name(kIndex), name) instance_id = 42 # Supply a random instance id. bundle.set_model_instance_id(kIndex, instance_id) self.assertEqual(bundle.get_model_instance_id(kIndex), instance_id)
def test_diagram_adder(self): system = CustomDiagram(2, 3) self.assertEqual(system.num_input_ports(), 2) with catch_drake_warnings(expected_count=1): system.get_num_input_ports() self.assertEqual(system.get_input_port(0).size(), 3) self.assertEqual(system.num_output_ports(), 1) with catch_drake_warnings(expected_count=1): system.get_num_output_ports() self.assertEqual(system.get_output_port(0).size(), 3)
def test_pose_vector(self): value = PoseVector() self.assertTrue(isinstance(value, BasicVector)) self.assertTrue(isinstance(copy.copy(value), PoseVector)) self.assertTrue(isinstance(value.Clone(), PoseVector)) self.assertEqual(value.size(), PoseVector.kSize) # - Accessors. with catch_drake_warnings(expected_count=1): self.assertTrue(isinstance(value.get_isometry(), Isometry3)) self.assertTrue(isinstance(value.get_transform(), RigidTransform)) self.assertTrue(isinstance( value.get_rotation(), Quaternion)) self.assertTrue(isinstance( value.get_translation(), np.ndarray)) # - Value. with catch_drake_warnings(expected_count=1): self.assertTrue(np.allclose( value.get_isometry().matrix(), np.eye(4, 4))) self.assertTrue(np.allclose( value.get_transform().GetAsMatrix4(), np.eye(4, 4))) # - Mutators. p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) X_expected = RigidTransform(quaternion=q, p=p) value.set_translation(p) value.set_rotation(q) with catch_drake_warnings(expected_count=1): self.assertTrue(np.allclose( value.get_isometry().matrix(), X_expected.GetAsMatrix4())) self.assertTrue(np.allclose( value.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4())) # - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz)) vector_expected = np.hstack((p, q.wxyz())) vector_actual = value.get_value() self.assertTrue(np.allclose(vector_actual, vector_expected)) # - Fully-parameterized constructor. value1 = PoseVector(rotation=q, translation=p) with catch_drake_warnings(expected_count=1): self.assertTrue(np.allclose( value1.get_isometry().matrix(), X_expected.GetAsMatrix4())) self.assertTrue(np.allclose( value1.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4())) # Test mutation via RigidTransform p2 = [10, 20, 30] q2 = Quaternion(wxyz=normalized([0.2, 0.3, 0.5, 0.8])) X2_expected = RigidTransform(quaternion=q2, p=p2) value.set_transform(X2_expected) self.assertTrue(np.allclose( value.get_transform().GetAsMatrix4(), X2_expected.GetAsMatrix4()))
def test_simulator_ctor(self): # Tests a simple simulation for supported scalar types. for T in [float, AutoDiffXd]: # Create simple system. system = ConstantVectorSource_[T]([1.]) def check_output(context): # Check number of output ports and value for a given context. output = system.AllocateOutput() self.assertEqual(output.num_ports(), 1) system.CalcOutput(context, output) if T == float: value = output.get_vector_data(0).get_value() self.assertTrue(np.allclose([1], value)) elif T == AutoDiffXd: value = output.get_vector_data(0)._get_value_copy() # TODO(eric.cousineau): Define `isfinite` ufunc, if # possible, to use for `np.allclose`. self.assertEqual(value.shape, (1,)) self.assertEqual(value[0], AutoDiffXd(1.)) else: raise RuntimeError("Bad T: {}".format(T)) # Create simulator with basic constructor. simulator = Simulator_[T](system) simulator.Initialize() simulator.set_target_realtime_rate(0) simulator.set_publish_every_time_step(True) self.assertTrue(simulator.get_context() is simulator.get_mutable_context()) check_output(simulator.get_context()) simulator.AdvanceTo(1) # Create simulator specifying context. context = system.CreateDefaultContext() with catch_drake_warnings(expected_count=1): context.set_time(0.) context.SetTime(0.) with catch_drake_warnings(expected_count=1): context.set_accuracy(1e-4) context.SetAccuracy(1e-4) self.assertEqual(context.get_accuracy(), 1e-4) # @note `simulator` now owns `context`. simulator = Simulator_[T](system, context) self.assertTrue(simulator.get_context() is context) check_output(context) simulator.AdvanceTo(1)
def test_AddAngleBetweenVectorsConstraint(self): na_A = np.array([0.2, -0.4, 0.9]) nb_B = np.array([1.4, -0.1, 1.8]) angle_lower = 0.2 * math.pi angle_upper = 0.2 * math.pi self.ik_two_bodies.AddAngleBetweenVectorsConstraint( frameA=self.body1_frame, na_A=na_A, frameB=self.body2_frame, nb_B=nb_B, angle_lower=angle_lower, angle_upper=angle_upper) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body2_quat = self._body2_quat(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() na_W = body1_rotmat.dot(na_A) nb_W = body2_rotmat.dot(nb_B) angle = math.acos(na_W.transpose().dot(nb_W) / (np.linalg.norm(na_W) * np.linalg.norm(nb_W))) self.assertLess(math.fabs(angle - angle_lower), 1E-6) with catch_drake_warnings(expected_count=2): self.assertEqual( self.prog.Solve(), mp.SolutionResult.kSolutionFound) self.assertTrue(np.allclose( self.prog.GetSolution(self.q), q_val))
def test_mock_lcm_induce_subscriber_callback_deprecated(self): dut = DrakeMockLcm() dut.Subscribe(channel="TEST_CHANNEL", handler=self._handler) with catch_drake_warnings(expected_count=1): dut.InduceSubscriberCallback( channel="TEST_CHANNEL", buffer=self.quat.encode()) self.assertEqual(self.count, 1)
def test_AddPositionConstraint(self): p_BQ = np.array([0.2, 0.3, 0.5]) p_AQ_lower = np.array([-0.1, -0.2, -0.3]) p_AQ_upper = np.array([-0.05, -0.12, -0.28]) self.ik_two_bodies.AddPositionConstraint( frameB=self.body1_frame, p_BQ=p_BQ, frameA=self.body2_frame, p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body1_pos = self._body1_xyz(q_val) body2_quat = self._body2_quat(q_val) body2_pos = self._body2_xyz(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() p_AQ = body2_rotmat.transpose().dot( body1_rotmat.dot(p_BQ) + body1_pos - body2_pos) self.assertTrue(np.greater(p_AQ, p_AQ_lower - 1E-6 * np.ones((3, 1))).all()) self.assertTrue(np.less(p_AQ, p_AQ_upper + 1E-6 * np.ones((3, 1))).all()) with catch_drake_warnings(expected_count=2): self.assertEqual( self.prog.Solve(), mp.SolutionResult.kSolutionFound) self.assertTrue(np.allclose( self.prog.GetSolution(self.q), q_val))
def test_deprecated_protected_aliases(self): """Tests a subset of protected aliases, pursuant to #9651.""" class OldSystem(LeafSystem): def __init__(self): LeafSystem.__init__(self) self.called_publish = False # Check a non-overridable method with catch_drake_warnings(expected_count=1): self._DeclareVectorInputPort("x", BasicVector(1)) def _DoPublish(self, context, events): self.called_publish = True # Ensure old overrides are still used system = OldSystem() context = system.CreateDefaultContext() with catch_drake_warnings(expected_count=1): system.Publish(context) self.assertTrue(system.called_publish) # Ensure documentation doesn't duplicate stuff. with catch_drake_warnings(expected_count=1): self.assertIn("deprecated", LeafSystem._DoPublish.__doc__) # This will warn both on (a) calling the method and (b) on the # invocation of the override. with catch_drake_warnings(expected_count=2): LeafSystem._DoPublish(system, context, []) class AccidentallyBothSystem(LeafSystem): def __init__(self): LeafSystem.__init__(self) self.called_old_publish = False self.called_new_publish = False def DoPublish(self, context, events): self.called_new_publish = True def _DoPublish(self, context, events): self.called_old_publish = True system = AccidentallyBothSystem() context = system.CreateDefaultContext() # This will trigger no deprecations, as the newer publish is called. system.Publish(context) self.assertTrue(system.called_new_publish) self.assertFalse(system.called_old_publish)
def test_deprecated_parsing(self): sdf_file = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(time_step=0.01) with catch_drake_warnings(expected_count=1): result = AddModelFromSdfFile(plant=plant, file_name=sdf_file) self.assertIsInstance(result, ModelInstanceIndex)
def test_ipopt_solver_deprecated(self): # This serves as the (only) unit test coverage for the deprecated # SolverBase method that writes the solution back into the program. prog, x, x_expected = self._make_prog() solver = IpoptSolver() with catch_drake_warnings(expected_count=2): result = solver.Solve(prog) solution = prog.GetSolution(x) self.assertEqual(result, mp.SolutionResult.kSolutionFound) self.assertTrue(np.allclose(solution, x_expected))
def test_lcm(self): dut = DrakeLcm() self.assertIsInstance(dut, DrakeLcmInterface) # Quickly start and stop the receiving thread. with catch_drake_warnings(expected_count=2): dut.StartReceiveThread() dut.StopReceiveThread() # Test virtual function names. dut.Publish dut.HandleSubscriptions
def DoHasDirectFeedthrough(self, input_port, output_port): # Test inputs. test.assertIn(input_port, [0, 1]) test.assertEqual(output_port, 0) # Call base method to ensure we do not get recursion. with catch_drake_warnings(expected_count=1): base_return = LeafSystem.DoHasDirectFeedthrough( self, input_port, output_port) test.assertTrue(base_return is None) # Return custom methods. self.called_feedthrough = True return False
def test_deprecated_tree_api(self): plant = MultibodyPlant() plant.Finalize() with catch_drake_warnings() as w: num_expected_warnings = [0] def expect_new_warning(msg_part): num_expected_warnings[0] += 1 self.assertEqual(len(w), num_expected_warnings[0]) self.assertIn(msg_part, str(w[-1].message)) tree = plant.tree() expect_new_warning("`tree()`") MobilizerIndex(0) expect_new_warning("`MobilizerIndex`") BodyNodeIndex(0) expect_new_warning("`BodyNodeIndex`") MultibodyForces(model=tree) expect_new_warning("`MultibodyForces(plant)`") element = plant.world_body() self.assertIsInstance(element.get_parent_tree(), MultibodyTree) expect_new_warning("`get_parent_tree()`") # Check old spellings (no deprecation warnings). self.check_old_spelling_exists(tree.CalcRelativeTransform) self.check_old_spelling_exists(tree.CalcPointsPositions) self.check_old_spelling_exists( tree.CalcFrameGeometricJacobianExpressedInWorld) self.check_old_spelling_exists(tree.EvalBodyPoseInWorld) self.check_old_spelling_exists(tree.SetFreeBodyPoseOrThrow) self.check_old_spelling_exists(tree.SetFreeBodySpatialVelocityOrThrow) self.check_old_spelling_exists(tree.EvalBodySpatialVelocityInWorld) self.check_old_spelling_exists(tree.GetPositionsFromArray) self.check_old_spelling_exists(tree.GetVelocitiesFromArray) self.check_old_spelling_exists(tree.CalcMassMatrixViaInverseDynamics) self.check_old_spelling_exists(tree.CalcBiasTerm) self.check_old_spelling_exists(tree.CalcInverseDynamics) self.check_old_spelling_exists(tree.num_frames) self.check_old_spelling_exists(tree.get_body) self.check_old_spelling_exists(tree.get_joint) self.check_old_spelling_exists(tree.get_joint_actuator) self.check_old_spelling_exists(tree.get_frame) self.check_old_spelling_exists(tree.GetModelInstanceName) context = plant.CreateDefaultContext() # All body poses. X_WB, = tree.CalcAllBodyPosesInWorld(context) self.assertIsInstance(X_WB, Isometry3) v_WB, = tree.CalcAllBodySpatialVelocitiesInWorld(context) self.assertIsInstance(v_WB, SpatialVelocity)
def test_eval_binding(self): qp = TestQP() prog = qp.prog x = qp.x x_expected = np.array([1., 1.]) costs = qp.costs cost_values_expected = [2., 1.] constraints = qp.constraints constraint_values_expected = [1., 1., 2., 3.] with catch_drake_warnings(action='ignore'): prog.Solve() self.assertTrue(np.allclose(prog.GetSolution(x), x_expected)) enum = zip(constraints, constraint_values_expected) for (constraint, value_expected) in enum: value = prog.EvalBindingAtSolution(constraint) self.assertTrue(np.allclose(value, value_expected)) enum = zip(costs, cost_values_expected) for (cost, value_expected) in enum: value = prog.EvalBindingAtSolution(cost) self.assertTrue(np.allclose(value, value_expected)) # Existence check for non-autodiff versions. self.assertIsInstance( prog.EvalBinding(costs[0], x_expected), np.ndarray) self.assertIsInstance( prog.EvalBindings(prog.GetAllConstraints(), x_expected), np.ndarray) # Existence check for autodiff versions. self.assertIsInstance( jacobian(partial(prog.EvalBinding, costs[0]), x_expected), np.ndarray) self.assertIsInstance( jacobian(partial(prog.EvalBindings, prog.GetAllConstraints()), x_expected), np.ndarray) # Bindings for `Eval`. x_list = (float(1.), AutoDiffXd(1.), sym.Variable("x")) T_y_list = (float, AutoDiffXd, sym.Expression) evaluator = costs[0].evaluator() for x_i, T_y_i in zip(x_list, T_y_list): y_i = evaluator.Eval(x=[x_i, x_i]) self.assertIsInstance(y_i[0], T_y_i)
def test_AddGazeTargetConstraint(self): p_AS = np.array([0.1, 0.2, 0.3]) n_A = np.array([0.3, 0.5, 1.2]) p_BT = np.array([1.1, 0.2, 1.5]) cone_half_angle = 0.2 * math.pi self.ik_two_bodies.AddGazeTargetConstraint( frameA=self.body1_frame, p_AS=p_AS, n_A=n_A, frameB=self.body2_frame, p_BT=p_BT, cone_half_angle=cone_half_angle) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body1_pos = self._body1_xyz(q_val) body2_quat = self._body2_quat(q_val) body2_pos = self._body2_xyz(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() p_WS = body1_pos + body1_rotmat.dot(p_AS) p_WT = body2_pos + body2_rotmat.dot(p_BT) p_ST_W = p_WT - p_WS n_W = body1_rotmat.dot(n_A) self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm( p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6) with catch_drake_warnings(expected_count=2): self.assertEqual( self.prog.Solve(), mp.SolutionResult.kSolutionFound) q_val = self.prog.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body1_pos = self._body1_xyz(q_val) body2_quat = self._body2_quat(q_val) body2_pos = self._body2_xyz(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() p_WS = body1_pos + body1_rotmat.dot(p_AS) p_WT = body2_pos + body2_rotmat.dot(p_BT) p_ST_W = p_WT - p_WS n_W = body1_rotmat.dot(n_A) self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm( p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6)
def test_sos(self): # Find a,b,c,d subject to # a(0) + a(1)*x, # b(0) + 2*b(1)*x + b(2)*x^2 is SOS, # c(0)*x^2 + 2*c(1)*x*y + c(2)*y^2 is SOS, # d(0)*x^2 is SOS. # d(1)*x^2 is SOS. prog = mp.MathematicalProgram() x = prog.NewIndeterminates(1, "x") poly = prog.NewFreePolynomial(sym.Variables(x), 1) (poly, binding) = prog.NewSosPolynomial( indeterminates=sym.Variables(x), degree=2) y = prog.NewIndeterminates(1, "y") (poly, binding) = prog.NewSosPolynomial( monomial_basis=(sym.Monomial(x[0]), sym.Monomial(y[0]))) d = prog.NewContinuousVariables(2, "d") prog.AddSosConstraint(d[0]*x.dot(x)) prog.AddSosConstraint(d[1]*x.dot(x), [sym.Monomial(x[0])]) result = mp.Solve(prog) self.assertTrue(result.is_success()) # Test SubstituteSolution(sym.Expression) with catch_drake_warnings(action='ignore'): prog.Solve() # TODO(eric.cousineau): Expose `SymbolicTestCase` so that other # tests can use the assertion utilities. self.assertEqual( prog.SubstituteSolution(d[0] + d[1]).Evaluate(), prog.GetSolution(d[0]) + prog.GetSolution(d[1])) # Test SubstituteSolution(sym.Polynomial) poly = d[0]*x.dot(x) poly_sub_actual = prog.SubstituteSolution( sym.Polynomial(poly, sym.Variables(x))) poly_sub_expected = sym.Polynomial( prog.SubstituteSolution(d[0])*x.dot(x), sym.Variables(x)) # TODO(soonho): At present, these must be converted to `Expression` # to compare, because as `Polynomial`s the comparison fails with # `0*x(0)^2` != `0`, which indicates that simplification is not # happening somewhere. self.assertTrue( poly_sub_actual.ToExpression().EqualTo( poly_sub_expected.ToExpression()), "{} != {}".format(poly_sub_actual, poly_sub_expected))
def _do_test_direct_transcription(self, use_deprecated_solve): # Integrator. plant = LinearSystem(A=[0.], B=[1.], C=[1.], D=[0.], time_period=0.1) context = plant.CreateDefaultContext() dirtran = DirectTranscription(plant, context, num_time_samples=21) # Spell out most of the methods, regardless of whether they make sense # as a consistent optimization. The goal is to check the bindings, # not the implementation. t = dirtran.time() dt = dirtran.fixed_timestep() x = dirtran.state() x2 = dirtran.state(2) x0 = dirtran.initial_state() xf = dirtran.final_state() u = dirtran.input() u2 = dirtran.input(2) dirtran.AddRunningCost(x.dot(x)) dirtran.AddConstraintToAllKnotPoints(u[0] == 0) dirtran.AddFinalCost(2*x.dot(x)) initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3*21], np.zeros((1, 2))) initial_x = PiecewisePolynomial() dirtran.SetInitialTrajectory(initial_u, initial_x) if use_deprecated_solve: with catch_drake_warnings(expected_count=6): dirtran.Solve() times = dirtran.GetSampleTimes() inputs = dirtran.GetInputSamples() states = dirtran.GetStateSamples() input_traj = dirtran.ReconstructInputTrajectory() state_traj = dirtran.ReconstructStateTrajectory() else: result = mp.Solve(dirtran) times = dirtran.GetSampleTimes(result) inputs = dirtran.GetInputSamples(result) states = dirtran.GetStateSamples(result) input_traj = dirtran.ReconstructInputTrajectory(result) state_traj = dirtran.ReconstructStateTrajectory(result)
def test_abstract_io_port(self): test = self # N.B. Since this has trivial operations, we can test all scalar types. for T in [float, AutoDiffXd, Expression]: default_value = ("default", T(0.)) expected_input_value = ("input", T(np.pi)) expected_output_value = ("output", 2*T(np.pi)) class CustomAbstractSystem(LeafSystem_[T]): def __init__(self): LeafSystem_[T].__init__(self) self.input_port = self.DeclareAbstractInputPort( "in", AbstractValue.Make(default_value)) self.output_port = self.DeclareAbstractOutputPort( "out", lambda: AbstractValue.Make(default_value), self.DoCalcAbstractOutput, prerequisites_of_calc=set([ self.input_port.ticket()])) def DoCalcAbstractOutput(self, context, y_data): input_value = self.EvalAbstractInput( context, 0).get_value() # The allocator function will populate the output with # the "input" test.assertTupleEqual(input_value, expected_input_value) y_data.set_value(expected_output_value) test.assertTupleEqual(y_data.get_value(), expected_output_value) system = CustomAbstractSystem() context = system.CreateDefaultContext() self.assertEqual(context.num_input_ports(), 1) context.FixInputPort(0, AbstractValue.Make(expected_input_value)) output = system.AllocateOutput() self.assertEqual(output.num_ports(), 1) with catch_drake_warnings(expected_count=1): output.get_num_ports() system.CalcOutput(context, output) value = output.get_data(0) self.assertEqual(value.get_value(), expected_output_value)
def test_AddMinimumDistanceConstraint(self): ik = self.ik_two_bodies W = self.plant.world_frame() B1 = self.body1_frame B2 = self.body2_frame min_distance = 0.1 tol = 1e-2 radius1 = 0.1 radius2 = 0.2 ik.AddMinimumDistanceConstraint(minimal_distance=min_distance) context = self.plant.CreateDefaultContext() R_I = np.eye(3) self.plant.SetFreeBodyPose( context, B1.body(), Isometry3(R_I, [0, 0, 0.01])) self.plant.SetFreeBodyPose( context, B2.body(), Isometry3(R_I, [0, 0, -0.01])) def get_min_distance_actual(): X = partial(self.plant.CalcRelativeTransform, context) distance = norm(X(W, B1).translation() - X(W, B2).translation()) return distance - radius1 - radius2 self.assertLess(get_min_distance_actual(), min_distance - tol) self.prog.SetInitialGuess(ik.q(), self.plant.GetPositions(context)) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(ik.q()) self.plant.SetPositions(context, q_val) self.assertGreater(get_min_distance_actual(), min_distance - tol) with catch_drake_warnings(expected_count=2): self.assertEqual( self.prog.Solve(), mp.SolutionResult.kSolutionFound) self.assertTrue(np.allclose( self.prog.GetSolution(ik.q()), q_val))
def test_AddOrientationConstraint(self): theta_bound = 0.2 * math.pi R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5)) R_BbarB = RotationMatrix( quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3)) self.ik_two_bodies.AddOrientationConstraint( frameAbar=self.body1_frame, R_AbarA=R_AbarA, frameBbar=self.body2_frame, R_BbarB=R_BbarB, theta_bound=theta_bound) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body2_quat = self._body2_quat(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat) R_AB = R_AbarA.matrix().transpose().dot( R_AbarBbar.dot(R_BbarB.matrix())) self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6) with catch_drake_warnings(expected_count=2): self.assertEqual( self.prog.Solve(), mp.SolutionResult.kSolutionFound) q_val = self.prog.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body2_quat = self._body2_quat(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat) R_AB = R_AbarA.matrix().transpose().dot( R_AbarBbar.dot(R_BbarB.matrix())) self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
def _fix_adder_inputs(self, context): self.assertEqual(context.num_input_ports(), 2) with catch_drake_warnings(expected_count=1): context.get_num_input_ports() context.FixInputPort(0, BasicVector([1, 2, 3])) context.FixInputPort(1, BasicVector([4, 5, 6]))
def test_multibody_plant_api_via_parsing(self): # TODO(eric.cousineau): Decouple this when construction can be done # without parsing. # This a subset of `multibody_plant_sdf_parser_test.cc`. file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(time_step=0.01) model_instance = Parser(plant).AddModelFromFile(file_name) self.assertIsInstance(model_instance, ModelInstanceIndex) plant.Finalize() benchmark = MakeAcrobotPlant(AcrobotParameters(), True) self.assertEqual(plant.num_bodies(), benchmark.num_bodies()) self.assertEqual(plant.num_joints(), benchmark.num_joints()) self.assertEqual(plant.num_actuators(), benchmark.num_actuators()) self.assertEqual( plant.num_model_instances(), benchmark.num_model_instances() + 1) self.assertEqual(plant.num_positions(), benchmark.num_positions()) self.assertEqual( plant.num_positions(model_instance=model_instance), benchmark.num_positions()) self.assertEqual( plant.num_velocities(), benchmark.num_velocities()) self.assertEqual( plant.num_multibody_states(), benchmark.num_multibody_states()) self.assertEqual( plant.num_actuated_dofs(), benchmark.num_actuated_dofs()) self.assertTrue(plant.is_finalized()) self.assertTrue(plant.HasBodyNamed(name="Link1")) self.assertTrue(plant.HasBodyNamed( name="Link1", model_instance=model_instance)) self.assertTrue(plant.HasJointNamed(name="ShoulderJoint")) self.assertTrue(plant.HasJointNamed( name="ShoulderJoint", model_instance=model_instance)) shoulder = plant.GetJointByName(name="ShoulderJoint") self._test_joint_api(shoulder) np.testing.assert_array_equal( shoulder.position_lower_limits(), [-np.inf]) np.testing.assert_array_equal( shoulder.position_upper_limits(), [np.inf]) self.assertIs(shoulder, plant.GetJointByName( name="ShoulderJoint", model_instance=model_instance)) self._test_joint_actuator_api( plant.GetJointActuatorByName(name="ElbowJoint")) self._test_body_api(plant.GetBodyByName(name="Link1")) self.assertIs( plant.GetBodyByName(name="Link1"), plant.GetBodyByName(name="Link1", model_instance=model_instance)) self.assertEqual(len(plant.GetBodyIndices(model_instance)), 2) self._test_frame_api(plant.GetFrameByName(name="Link1")) self.assertIs( plant.GetFrameByName(name="Link1"), plant.GetFrameByName(name="Link1", model_instance=model_instance)) self.assertEqual( model_instance, plant.GetModelInstanceByName(name="acrobot")) self.assertIsInstance( plant.get_actuation_input_port(), InputPort) self.assertIsInstance( plant.get_state_output_port(), OutputPort) # Smoke test of deprecated methods. with catch_drake_warnings(expected_count=2): plant.get_continuous_state_output_port() plant.get_continuous_state_output_port(model_instance) self.assertIsInstance( plant.get_contact_results_output_port(), OutputPort) self.assertIsInstance(plant.num_frames(), int) self.assertIsInstance(plant.get_body(body_index=BodyIndex(0)), Body) self.assertIs(shoulder, plant.get_joint(joint_index=JointIndex(0))) self.assertIsInstance(plant.get_joint_actuator( actuator_index=JointActuatorIndex(0)), JointActuator) self.assertIsInstance( plant.get_frame(frame_index=FrameIndex(0)), Frame) self.assertEqual("acrobot", plant.GetModelInstanceName( model_instance=model_instance))
from six import text_type as unicode import numpy as np from pydrake.common import FindResourceOrThrow from pydrake.common.eigen_geometry import Isometry3 from pydrake.common.test_utilities.deprecation import catch_drake_warnings from pydrake.systems.framework import InputPort, OutputPort from pydrake.math import RigidTransform, RollPitchYaw from pydrake.systems.lcm import LcmPublisherSystem # Deprecated modules. # N.B. Place `with` afterwards to avoid needing to place `#noqa` on other # modules. Additionally, assert length of warnings here rather than in the test # so that we do not have to worry about whether a module has already been # loaded. with catch_drake_warnings(expected_count=3): from pydrake.multibody.multibody_tree import ( BodyNodeIndex, MobilizerIndex, MultibodyTree, ) from pydrake.multibody.multibody_tree.parsing import AddModelFromSdfFile def get_index_class(cls): # Maps a class to its corresponding index class, accommdating inheritance. class_to_index_class_map = { Body: BodyIndex, ForceElement: ForceElementIndex, Frame: FrameIndex, Joint: JointIndex,
def test_mock_lcm_get_last_published_message_deprecated(self): dut = DrakeMockLcm() dut.Publish(channel="TEST_CHANNEL", buffer=self.quat.encode()) with catch_drake_warnings(expected_count=1): raw = dut.get_last_published_message("TEST_CHANNEL") self.assertEqual(raw, self.quat.encode())
def __init__(self): LeafSystem.__init__(self) self.called_publish = False # Check a non-overridable method with catch_drake_warnings(expected_count=1): self._DeclareVectorInputPort("x", BasicVector(1))
def test_context_api(self): # Capture miscellaneous functions not yet tested. model_value = AbstractValue.Make("Hello") model_vector = BasicVector([1., 2.]) class TrivialSystem(LeafSystem): def __init__(self): LeafSystem.__init__(self) self.DeclareContinuousState(1) self.DeclareDiscreteState(2) self.DeclareAbstractState(model_value.Clone()) self.DeclareAbstractParameter(model_value.Clone()) self.DeclareNumericParameter(model_vector.Clone()) system = TrivialSystem() context = system.CreateDefaultContext() self.assertTrue( context.get_state() is context.get_mutable_state()) self.assertEqual(context.num_continuous_states(), 1) self.assertTrue( context.get_continuous_state_vector() is context.get_mutable_continuous_state_vector()) self.assertEqual(context.num_discrete_state_groups(), 1) with catch_drake_warnings(expected_count=1): context.get_num_discrete_state_groups() self.assertTrue( context.get_discrete_state_vector() is context.get_mutable_discrete_state_vector()) self.assertTrue( context.get_discrete_state(0) is context.get_discrete_state_vector()) self.assertTrue( context.get_discrete_state(0) is context.get_discrete_state().get_vector(0)) self.assertTrue( context.get_mutable_discrete_state(0) is context.get_mutable_discrete_state_vector()) self.assertTrue( context.get_mutable_discrete_state(0) is context.get_mutable_discrete_state().get_vector(0)) self.assertEqual(context.num_abstract_states(), 1) with catch_drake_warnings(expected_count=1): context.get_num_abstract_states() self.assertTrue( context.get_abstract_state() is context.get_mutable_abstract_state()) self.assertTrue( context.get_abstract_state(0) is context.get_mutable_abstract_state(0)) self.assertEqual( context.get_abstract_state(0).get_value(), model_value.get_value()) # Check abstract state API (also test AbstractValues). values = context.get_abstract_state() self.assertEqual(values.size(), 1) self.assertEqual( values.get_value(0).get_value(), model_value.get_value()) self.assertEqual( values.get_mutable_value(0).get_value(), model_value.get_value()) values.SetFrom(values.Clone()) with catch_drake_warnings(expected_count=1): values.CopyFrom(values.Clone()) # Check parameter accessors. self.assertEqual(system.num_abstract_parameters(), 1) self.assertEqual( context.get_abstract_parameter(index=0).get_value(), model_value.get_value()) self.assertEqual(system.num_numeric_parameter_groups(), 1) np.testing.assert_equal( context.get_numeric_parameter(index=0).get_value(), model_vector.get_value()) # Check diagram context accessors. builder = DiagramBuilder() builder.AddSystem(system) diagram = builder.Build() context = diagram.CreateDefaultContext() # Existence check. self.assertIsNot( diagram.GetMutableSubsystemState(system, context), None) subcontext = diagram.GetMutableSubsystemContext(system, context) self.assertIsNot(subcontext, None) self.assertIs( diagram.GetSubsystemContext(system, context), subcontext)
def _do_test_direct_collocation(self, use_deprecated_solve): plant = PendulumPlant() context = plant.CreateDefaultContext() dircol = DirectCollocation( plant, context, num_time_samples=21, minimum_timestep=0.2, maximum_timestep=0.5, input_port_index=InputPortSelection.kUseFirstInputIfItExists, assume_non_continuous_states_are_fixed=False) # Spell out most of the methods, regardless of whether they make sense # as a consistent optimization. The goal is to check the bindings, # not the implementation. t = dircol.time() dt = dircol.timestep(0) x = dircol.state() x2 = dircol.state(2) x0 = dircol.initial_state() xf = dircol.final_state() u = dircol.input() u2 = dircol.input(2) dircol.AddRunningCost(x.dot(x)) dircol.AddConstraintToAllKnotPoints(u[0] == 0) dircol.AddTimeIntervalBounds(0.3, 0.4) dircol.AddEqualTimeIntervalsConstraints() dircol.AddDurationBounds(.3*21, 0.4*21) dircol.AddFinalCost(2*x.dot(x)) initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3*21], np.zeros((1, 2))) initial_x = PiecewisePolynomial() dircol.SetInitialTrajectory(initial_u, initial_x) global input_was_called input_was_called = False global state_was_called state_was_called = False def input_callback(t, u): global input_was_called input_was_called = True def state_callback(t, x): global state_was_called state_was_called = True dircol.AddInputTrajectoryCallback(input_callback) dircol.AddStateTrajectoryCallback(state_callback) if use_deprecated_solve: with catch_drake_warnings(expected_count=1): dircol.Solve() result = None else: result = mp.Solve(dircol) self.assertTrue(input_was_called) self.assertTrue(state_was_called) if use_deprecated_solve: with catch_drake_warnings(expected_count=5): times = dircol.GetSampleTimes() inputs = dircol.GetInputSamples() states = dircol.GetStateSamples() input_traj = dircol.ReconstructInputTrajectory() state_traj = dircol.ReconstructStateTrajectory() else: times = dircol.GetSampleTimes(result) inputs = dircol.GetInputSamples(result) states = dircol.GetStateSamples(result) input_traj = dircol.ReconstructInputTrajectory(result) state_traj = dircol.ReconstructStateTrajectory(result) constraint = DirectCollocationConstraint(plant, context) AddDirectCollocationConstraint(constraint, dircol.timestep(0), dircol.state(0), dircol.state(1), dircol.input(0), dircol.input(1), dircol)
def __init__(self): LeafSystem_[float].__init__(self) with catch_drake_warnings(expected_count=1): self.DeclareAbstractInputPort("in") self.DeclareVectorOutputPort("out", BasicVector(1), self._Out)
def test_bindings(self): qp = TestQP() prog = qp.prog x = qp.x self.assertEqual(prog.FindDecisionVariableIndices(vars=[x[0], x[1]]), [0, 1]) for binding in prog.GetAllCosts(): self.assertIsInstance(binding.evaluator(), mp.Cost) for binding in prog.GetLinearConstraints(): self.assertIsInstance(binding.evaluator(), mp.Constraint) for binding in prog.GetAllConstraints(): self.assertIsInstance(binding.evaluator(), mp.Constraint) self.assertTrue(prog.linear_costs()) for (i, binding) in enumerate(prog.linear_costs()): cost = binding.evaluator() self.assertTrue(np.allclose(cost.a(), np.ones((1, 2)))) self.assertTrue(prog.quadratic_costs()) for (i, binding) in enumerate(prog.quadratic_costs()): cost = binding.evaluator() self.assertTrue(np.allclose(cost.Q(), np.eye(2))) self.assertTrue(np.allclose(cost.b(), np.zeros(2))) self.assertTrue(prog.bounding_box_constraints()) for (i, binding) in enumerate(prog.bounding_box_constraints()): constraint = binding.evaluator() self.assertEqual( prog.FindDecisionVariableIndex(var=binding.variables()[0]), prog.FindDecisionVariableIndex(var=x[i])) num_constraints = constraint.num_constraints() if num_constraints == 1: self.assertEqual(constraint.A(), 1) self.assertEqual(constraint.lower_bound(), 1) self.assertEqual(constraint.upper_bound(), np.inf) else: self.assertTrue(np.allclose(constraint.A(), np.eye(2))) self.assertTrue(np.allclose(constraint.lower_bound(), [1, -np.inf])) self.assertTrue(np.allclose(constraint.upper_bound(), [np.inf, 2])) self.assertTrue(prog.linear_constraints()) for (i, binding) in enumerate(prog.linear_constraints()): constraint = binding.evaluator() self.assertEqual( prog.FindDecisionVariableIndex(var=binding.variables()[0]), prog.FindDecisionVariableIndex(var=x[0])) self.assertEqual( prog.FindDecisionVariableIndex(var=binding.variables()[1]), prog.FindDecisionVariableIndex(var=x[1])) self.assertTrue(np.allclose(constraint.A(), [3, -1])) self.assertTrue(constraint.lower_bound(), -2) self.assertTrue(constraint.upper_bound(), np.inf) self.assertTrue(prog.linear_equality_constraints()) for (i, binding) in enumerate(prog.linear_equality_constraints()): constraint = binding.evaluator() self.assertEqual( prog.FindDecisionVariableIndex(var=binding.variables()[0]), prog.FindDecisionVariableIndex(var=x[0])) self.assertEqual( prog.FindDecisionVariableIndex(var=binding.variables()[1]), prog.FindDecisionVariableIndex(var=x[1])) self.assertTrue(np.allclose(constraint.A(), [1, 2])) self.assertTrue(constraint.lower_bound(), 3) self.assertTrue(constraint.upper_bound(), 3) result = mp.Solve(prog) self.assertTrue(result.is_success()) x_expected = np.array([1, 1]) self.assertTrue(np.allclose(result.GetSolution(x), x_expected)) # Test deprecated method. with catch_drake_warnings(expected_count=1): c = binding.constraint()
def test_multibody_gravity_default(self): plant = MultibodyPlant() # Smoke test of deprecated methods. with catch_drake_warnings(expected_count=1): plant.AddForceElement(UniformGravityFieldElement()) plant.Finalize()
def test_frame_pose_vector_api(self): mut.FramePoseVector() with catch_drake_warnings(expected_count=1): mut.FramePoseVector(source_id=mut.SourceId.get_new_id(), ids=[mut.FrameId.get_new_id()])