def test_mbp_overloads(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(0.0)
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
frame = plant.GetFrameByName("Link2")
parameters = mut.DifferentialInverseKinematicsParameters(2, 2)
mut.DoDifferentialInverseKinematics(plant, context, np.zeros(6), frame,
parameters)
mut.DoDifferentialInverseKinematics(plant, context, RigidTransform(),
frame, parameters)
def test_mbp_overloads(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
AddModelFromSdfFile(file_name=file_name, plant=plant, scene_graph=None)
plant.Finalize()
context = plant.CreateDefaultContext()
frame = plant.GetFrameByName("Link2")
parameters = mut.DifferentialInverseKinematicsParameters(2, 2)
mut.DoDifferentialInverseKinematics(plant, context, np.zeros(6), frame,
parameters)
mut.DoDifferentialInverseKinematics(plant, context,
Isometry3.Identity(), frame,
parameters)
def test_api(self):
# Test existence for enumeration.
enum = mut.DifferentialInverseKinematicsStatus
enum.kSolutionFound
enum.kNoSolutionFound
enum.kStuck
# Test results.
results_cls = mut.DifferentialInverseKinematicsResult
obj = results_cls(joint_velocities=[0, 1], status=enum.kSolutionFound)
self.assertTrue((obj.joint_velocities == [0, 1]).all())
self.assertEqual(obj.status, enum.kSolutionFound)
# Test parameters.
params_cls = mut.DifferentialInverseKinematicsParameters
params = params_cls(num_positions=2, num_velocities=2)
# Test existence.
params.get_timestep
params.set_timestep
params.get_num_positions
params.get_nominal_joint_position
params.get_num_velocities
params.get_nominal_joint_position
params.set_nominal_joint_position
params.get_end_effector_velocity_gain
params.set_end_effector_velocity_gain
params.get_unconstrained_degrees_of_freedom_velocity_limit
params.set_unconstrained_degrees_of_freedom_velocity_limit
params.get_joint_position_limits
params.set_joint_position_limits
params.get_joint_velocity_limits
params.set_joint_velocity_limits
params.get_joint_acceleration_limits
params.set_joint_acceleration_limits
# Test a basic call for the API. These values intentionally have no
# physical meaning.
result = mut.DoDifferentialInverseKinematics(q_current=[0, 1],
v_current=[2, 3],
V=[0, 1, 2, 3, 4, 5],
J=np.array([
[0, 1, 2, 3, 4, 5],
[6, 7, 8, 9, 10, 11],
]).T,
parameters=params)
self.assertTrue(
np.allclose(result.joint_velocities, [1, 0], atol=1e-8, rtol=0))
self.assertEqual(result.status, enum.kSolutionFound)
