def test_points_on_radius(self):
ds = CartesianRingDS()
ds.set_parameter(sr.Parameter("center", self.center, sr.StateType.PARAMETER_CARTESIANPOSE))
ds.set_parameter(sr.Parameter("radius", self.radius, sr.StateType.PARAMETER_DOUBLE))
ds.set_parameter(sr.Parameter("width", self.width, sr.StateType.PARAMETER_DOUBLE))
ds.set_parameter(sr.Parameter("speed", self.speed, sr.StateType.PARAMETER_DOUBLE))
current_pose = copy.deepcopy(self.center)
twist = sr.CartesianTwist(ds.evaluate(current_pose))
self.assertTrue(np.linalg.norm(twist.data()) < self.tol)
current_pose.set_position(self.radius, 0, 0)
twist = sr.CartesianTwist(ds.evaluate(current_pose))
self.assertTrue(twist.get_linear_velocity()[0] < self.tol)
self.assertTrue(abs(twist.get_linear_velocity()[1] - self.speed) < self.tol)
current_pose.set_position(0, self.radius, 0)
twist = sr.CartesianTwist(ds.evaluate(current_pose))
self.assertTrue(abs(twist.get_linear_velocity()[0] + self.speed) < self.tol)
self.assertTrue(twist.get_linear_velocity()[1] < self.tol)
current_pose.set_position(-self.radius, 0, 0)
twist = sr.CartesianTwist(ds.evaluate(current_pose))
self.assertTrue(twist.get_linear_velocity()[0] < self.tol)
self.assertTrue(abs(twist.get_linear_velocity()[1] + self.speed) < self.tol)
current_pose.set_position(0, -self.radius, 0)
twist = sr.CartesianTwist(ds.evaluate(current_pose))
self.assertTrue(abs(twist.get_linear_velocity()[0] - self.speed) < self.tol)
self.assertTrue(twist.get_linear_velocity()[1] < self.tol)
current_pose.set_position(self.radius, 0, 1)
twist = sr.CartesianTwist(ds.evaluate(current_pose))
self.assertTrue(abs(twist.get_linear_velocity()[2] + 1) < self.tol)
def test_param_map(self):
param_dict = {
"int": sr.Parameter("int", 1, sr.StateType.PARAMETER_INT),
"double": sr.Parameter("double", 2.2,
sr.StateType.PARAMETER_DOUBLE),
"string": sr.Parameter("string", "test",
sr.StateType.PARAMETER_STRING)
}
param_list = [param for name, param in param_dict.items()]
m = sr.ParameterMap()
for name, param in param_dict.items():
m.set_parameter(param)
self.param_map_equal(param_dict, m)
m = sr.ParameterMap()
for name, param in param_dict.items():
m.set_parameter_value(param.get_name(), param.get_value(),
param.get_type())
self.param_map_equal(param_dict, m)
m = sr.ParameterMap()
m.set_parameters(param_dict)
self.param_map_equal(param_dict, m)
m = sr.ParameterMap()
m.set_parameters(param_list)
self.param_map_equal(param_dict, m)
m = sr.ParameterMap(param_dict)
self.param_map_equal(param_dict, m)
m = sr.ParameterMap(param_list)
self.param_map_equal(param_dict, m)
def test_param_copy(self):
param = sr.Parameter("test", 1, sr.StateType.PARAMETER_INT)
self.assertEqual(param.get_name(), "test")
self.assertEqual(param.get_type(), sr.StateType.PARAMETER_INT)
self.assertEqual(param.get_value(), 1)
param_copy = sr.Parameter(param)
self.assertEqual(param_copy.get_name(), "test")
self.assertEqual(param_copy.get_type(), sr.StateType.PARAMETER_INT)
self.assertEqual(param_copy.get_value(), 1)
def test_param_int(self):
param = sr.Parameter("int", sr.StateType.PARAMETER_INT)
self.assertTrue(param.is_empty())
self.assertEqual(param.get_name(), "int")
self.assertEqual(param.get_type(), sr.StateType.PARAMETER_INT)
param.set_value(1)
self.assertEqual(param.get_value(), 1)
param1 = sr.Parameter("int", 1, sr.StateType.PARAMETER_INT)
self.assertEqual(param1.get_value(), 1)
def test_param_double(self):
param = sr.Parameter("double", sr.StateType.PARAMETER_DOUBLE)
self.assertTrue(param.is_empty())
self.assertEqual(param.get_name(), "double")
self.assertEqual(param.get_type(), sr.StateType.PARAMETER_DOUBLE)
param.set_value(1.5)
self.assertEqual(param.get_value(), 1.5)
param1 = sr.Parameter("double", 1.5, sr.StateType.PARAMETER_DOUBLE)
self.assertEqual(param1.get_value(), 1.5)
def test_param_bool(self):
param = sr.Parameter("bool", sr.StateType.PARAMETER_BOOL)
self.assertTrue(param.is_empty())
self.assertEqual(param.get_name(), "bool")
self.assertEqual(param.get_type(), sr.StateType.PARAMETER_BOOL)
param.set_value(False)
self.assertEqual(param.get_value(), False)
param1 = sr.Parameter("bool", False, sr.StateType.PARAMETER_BOOL)
self.assertEqual(param1.get_value(), False)
def test_param_string(self):
param = sr.Parameter("string", sr.StateType.PARAMETER_STRING)
self.assertTrue(param.is_empty())
self.assertEqual(param.get_name(), "string")
self.assertEqual(param.get_type(), sr.StateType.PARAMETER_STRING)
param.set_value("parameter")
self.assertEqual(param.get_value(), "parameter")
param1 = sr.Parameter("string", "parameter",
sr.StateType.PARAMETER_STRING)
self.assertEqual(param1.get_value(), "parameter")
def test_param_matrix(self):
param = sr.Parameter("matrix", sr.StateType.PARAMETER_MATRIX)
self.assertTrue(param.is_empty())
self.assertEqual(param.get_name(), "matrix")
self.assertEqual(param.get_type(), sr.StateType.PARAMETER_MATRIX)
values = np.random.rand(3, 2)
param.set_value(values)
self.assert_np_array_equal(param.get_value(), values)
param1 = sr.Parameter("matrix", values, sr.StateType.PARAMETER_MATRIX)
self.assert_np_array_equal(param1.get_value(), values)
def test_param_vector(self):
param = sr.Parameter("vector", sr.StateType.PARAMETER_VECTOR)
self.assertTrue(param.is_empty())
self.assertEqual(param.get_name(), "vector")
self.assertEqual(param.get_type(), sr.StateType.PARAMETER_VECTOR)
values = np.random.rand(3)
param.set_value(values)
self.assert_np_array_equal(param.get_value(), values)
param1 = sr.Parameter("vector", values, sr.StateType.PARAMETER_VECTOR)
self.assert_np_array_equal(param1.get_value(), values)
def test_param_joint_state(self):
param = sr.Parameter("joint_state", sr.StateType.PARAMETER_JOINTSTATE)
self.assertTrue(param.is_empty())
self.assertEqual(param.get_name(), "joint_state")
self.assertEqual(param.get_type(), sr.StateType.PARAMETER_JOINTSTATE)
values = sr.JointState.Random("test", 3)
param.set_value(values)
self.joint_equal(param.get_value(), values)
param1 = sr.Parameter("joint_state", values,
sr.StateType.PARAMETER_JOINTSTATE)
self.joint_equal(param1.get_value(), values)
def test_param_ellipsoid(self):
param = sr.Parameter("ellipse", sr.StateType.PARAMETER_ELLIPSOID)
self.assertTrue(param.is_empty())
self.assertEqual(param.get_name(), "ellipse")
self.assertEqual(param.get_type(), sr.StateType.PARAMETER_ELLIPSOID)
values = sr.Ellipsoid("test")
param.set_value(values)
self.assertTrue(param.get_value().get_name(), values.get_name())
param1 = sr.Parameter("ellipse", values,
sr.StateType.PARAMETER_ELLIPSOID)
self.assertTrue(param1.get_value().get_name(), values.get_name())
def test_param_joint_positions(self):
param = sr.Parameter("joint_positions",
sr.StateType.PARAMETER_JOINTPOSITIONS)
self.assertTrue(param.is_empty())
self.assertEqual(param.get_name(), "joint_positions")
self.assertEqual(param.get_type(),
sr.StateType.PARAMETER_JOINTPOSITIONS)
values = sr.JointPositions.Random("test", 3)
param.set_value(values)
self.joint_equal(param.get_value(), values)
param1 = sr.Parameter("joint_positions", values,
sr.StateType.PARAMETER_JOINTPOSITIONS)
self.joint_equal(param1.get_value(), values)
def test_param_cartesian_pose(self):
param = sr.Parameter("cartesian_pose",
sr.StateType.PARAMETER_CARTESIANPOSE)
self.assertTrue(param.is_empty())
self.assertEqual(param.get_name(), "cartesian_pose")
self.assertEqual(param.get_type(),
sr.StateType.PARAMETER_CARTESIANPOSE)
values = sr.CartesianPose.Random("test")
param.set_value(values)
self.cartesian_equal(param.get_value(), values)
param1 = sr.Parameter("cartesian_pose", values,
sr.StateType.PARAMETER_CARTESIANPOSE)
self.cartesian_equal(param1.get_value(), values)
def test_convergence_on_radius_random_center(self):
center = sr.CartesianPose.Random("A")
ds = CartesianRingDS()
ds.set_parameter(sr.Parameter("center", center, sr.StateType.PARAMETER_CARTESIANPOSE))
ds.set_parameter(sr.Parameter("radius", self.radius, sr.StateType.PARAMETER_DOUBLE))
current_pose = sr.CartesianPose("B", self.radius * np.random.rand(3, 1))
for i in range(self.nb_steps):
twist = sr.CartesianTwist(ds.evaluate(current_pose))
twist.clamp(10, 10, 0.001, 0.001)
current_pose += self.dt * twist
self.assertTrue(
abs(np.linalg.norm(current_pose.get_position() - center.get_position()) - self.radius) < self.tol)
def test_is_compatible(self):
ds = CartesianPointAttractorDS()
state1 = sr.CartesianState.Identity("B", "A")
state2 = sr.CartesianState.Identity("D", "C")
state3 = sr.CartesianState.Identity("C", "A")
state4 = sr.CartesianState.Identity("C", "B")
with self.assertRaises(RuntimeError):
ds.evaluate(state1)
ds.set_base_frame(state1)
with self.assertRaises(RuntimeError):
ds.evaluate(state2)
with self.assertRaises(RuntimeError):
ds.evaluate(state3)
with self.assertRaises(RuntimeError):
ds.evaluate(state4)
ds.set_parameter(
sr.Parameter("attractor",
sr.CartesianState.Identity("CAttractor", "A"),
sr.StateType.PARAMETER_CARTESIANSTATE))
self.assertTrue(ds.is_compatible(state1))
self.assertFalse(ds.is_compatible(state2))
self.assertTrue(ds.is_compatible(state3))
self.assertTrue(ds.is_compatible(state4))
def test_stacked_moving_reference_frames(self):
AinWorld = sr.CartesianState.Random("A")
BinA = sr.CartesianState.Random("B", "A")
CinA = sr.CartesianState(sr.CartesianPose.Random("C", "A"))
ds = CartesianPointAttractorDS()
ds.set_parameter(
sr.Parameter("attractor", BinA,
sr.StateType.PARAMETER_CARTESIANSTATE))
twist = sr.CartesianTwist(ds.evaluate(CinA))
CinA.set_linear_velocity(np.random.rand(3, 1))
CinA.set_angular_velocity(np.random.rand(3, 1))
twist2 = sr.CartesianTwist(ds.evaluate(CinA))
self.assertTrue(
np.linalg.norm(twist.data()) -
np.linalg.norm(twist2.data()) < 1e-5)
twist = AinWorld * ds.evaluate(CinA)
ds.set_base_frame(AinWorld)
CinWorld = AinWorld * CinA
twist2 = ds.evaluate(CinWorld)
self.assertEqual(twist.get_reference_frame(),
AinWorld.get_reference_frame())
self.assertEqual(twist.get_reference_frame(),
twist2.get_reference_frame())
self.assertTrue(
np.linalg.norm(twist.data()) -
np.linalg.norm(twist2.data()) < 1e-5)
def test_param_int_array(self):
param = sr.Parameter("int_array", sr.StateType.PARAMETER_INT_ARRAY)
self.assertTrue(param.is_empty())
self.assertEqual(param.get_name(), "int_array")
self.assertEqual(param.get_type(), sr.StateType.PARAMETER_INT_ARRAY)
values = [2, 3, 4]
param.set_value(values)
[
self.assertEqual(param.get_value()[i], values[i])
for i in range(len(values))
]
param1 = sr.Parameter("int_array", values,
sr.StateType.PARAMETER_INT_ARRAY)
[
self.assertEqual(param1.get_value()[i], values[i])
for i in range(len(values))
]
def test_param_bool_array(self):
param = sr.Parameter("bool_array", sr.StateType.PARAMETER_BOOL_ARRAY)
self.assertTrue(param.is_empty())
self.assertEqual(param.get_name(), "bool_array")
self.assertEqual(param.get_type(), sr.StateType.PARAMETER_BOOL_ARRAY)
values = [True, False, False]
param.set_value(values)
[
self.assertEqual(param.get_value()[i], values[i])
for i in range(len(values))
]
param1 = sr.Parameter("bool_array", values,
sr.StateType.PARAMETER_BOOL_ARRAY)
[
self.assertEqual(param1.get_value()[i], values[i])
for i in range(len(values))
]
def test_controller_with_params(self):
param_list = [
sr.Parameter("damping", 0.0, sr.StateType.PARAMETER_DOUBLE),
sr.Parameter("stiffness", 5.0, sr.StateType.PARAMETER_DOUBLE),
sr.Parameter("inertia", 10.0, sr.StateType.PARAMETER_DOUBLE)
]
ctrl = create_cartesian_controller(CONTROLLER_TYPE.IMPEDANCE,
param_list)
self.assertFalse(ctrl is None)
self.assertEqual(ctrl.get_parameter_value("damping").size, 6 * 6)
self.assertEqual(ctrl.get_parameter_value("stiffness").size, 6 * 6)
self.assertEqual(ctrl.get_parameter_value("inertia").size, 6 * 6)
self.assertEqual(ctrl.get_parameter_value("damping").sum(), 0.0 * 6)
self.assertEqual(ctrl.get_parameter_value("stiffness").sum(), 5.0 * 6)
self.assertEqual(ctrl.get_parameter_value("inertia").sum(), 10.0 * 6)
def test_param_string_array(self):
param = sr.Parameter("string_array",
sr.StateType.PARAMETER_STRING_ARRAY)
self.assertTrue(param.is_empty())
self.assertEqual(param.get_name(), "string_array")
self.assertEqual(param.get_type(), sr.StateType.PARAMETER_STRING_ARRAY)
values = ["test", "parameter", "bindings"]
param.set_value(values)
[
self.assertEqual(param.get_value()[i], values[i])
for i in range(len(values))
]
param1 = sr.Parameter("string_array", values,
sr.StateType.PARAMETER_STRING_ARRAY)
[
self.assertEqual(param1.get_value()[i], values[i])
for i in range(len(values))
]
def test_empty_constructor(self):
ds = JointPointAttractorDS()
attractor = sr.JointState.Zero("robot", 3)
self.assertTrue(ds.get_parameter_value("attractor").is_empty())
self.assertTrue(ds.get_base_frame().is_empty())
ds.set_parameter(
sr.Parameter("attractor", attractor,
sr.StateType.PARAMETER_JOINTSTATE))
self.assertFalse(ds.get_parameter_value("attractor").is_empty())
def test_cartesian_factory(self):
cart_ds = create_cartesian_ds(DYNAMICAL_SYSTEM.NONE)
cart_ds.set_base_frame(sr.CartesianState.Identity("A"))
cart_ds.evaluate(sr.CartesianState.Identity("C", "A"))
self.assertTrue(cart_ds.evaluate(sr.CartesianState.Identity("C", "A")).is_empty())
self.assertTrue(len(cart_ds.get_parameter_list()) == 0)
param_list = [sr.Parameter("test", 1.0, sr.StateType.PARAMETER_DOUBLE)]
with self.assertRaises(RuntimeError):
cart_ds.set_parameters(param_list)
def test_joint_factory(self):
joint_ds = create_joint_ds(DYNAMICAL_SYSTEM.NONE)
joint_ds.set_base_frame(sr.JointState.Zero("robot", 3))
joint_ds.evaluate(sr.JointState.Random("robot", 3))
self.assertTrue(joint_ds.evaluate(sr.JointState.Random("robot", 3)).is_empty())
self.assertTrue(len(joint_ds.get_parameter_list()) == 0)
param_list = [sr.Parameter("test", 1.0, sr.StateType.PARAMETER_DOUBLE)]
with self.assertRaises(RuntimeError):
joint_ds.set_parameters(param_list)
def test_empty_constructor(self):
ds = CartesianRingDS()
center = sr.CartesianPose.Identity("CAttractor", "A")
self.assertTrue(ds.get_parameter_value("center").is_empty())
ds.set_parameter(sr.Parameter("center", center, sr.StateType.PARAMETER_CARTESIANPOSE))
self.assertFalse(ds.get_parameter_value("center").is_empty())
self.assertFalse(ds.get_base_frame().is_empty())
self.assertEqual(ds.get_base_frame().get_name(), center.get_reference_frame())
self.assertEqual(ds.get_base_frame().get_reference_frame(), center.get_reference_frame())
self.assert_np_array_equal(ds.get_base_frame().get_transformation_matrix(), np.eye(4))
def test_empty_constructor(self):
ds = CartesianCircularDS()
self.assertTrue(ds.get_parameter_value("limit_cycle").get_center_state().is_empty())
self.assertTrue(ds.get_base_frame().is_empty())
ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.StateType.PARAMETER_ELLIPSOID))
self.assertFalse(ds.get_parameter_value("limit_cycle").get_center_state().is_empty())
self.assertFalse(ds.get_base_frame().is_empty())
self.assertEqual(ds.get_base_frame().get_name(), self.center.get_reference_frame())
self.assertEqual(ds.get_base_frame().get_reference_frame(), self.center.get_reference_frame())
self.assert_np_array_equal(ds.get_base_frame().get_transformation_matrix(), np.eye(4))
def test_points_on_radius_random_center(self):
ds = CartesianCircularDS()
self.limit_cycle.set_center_position(np.random.rand(3, 1))
ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.StateType.PARAMETER_ELLIPSOID))
current_pose = sr.CartesianPose("A", 10 * np.random.rand(3, 1))
for i in range(self.nb_steps):
twist = sr.CartesianTwist(ds.evaluate(current_pose))
twist.clamp(10, 10, 0.001, 0.001)
current_pose += self.dt * twist
self.assertTrue(current_pose.dist(self.limit_cycle.get_center_pose(),
sr.CartesianStateVariable.POSITION) - self.radius < self.tol)
def test_convergence(self):
ds = JointPointAttractorDS()
attractor = sr.JointPositions.Random("robot", 3)
ds.set_parameter(
sr.Parameter("attractor", attractor,
sr.StateType.PARAMETER_JOINTSTATE))
current_state = sr.JointPositions.Random("robot", 3)
current_state.set_data(10 * current_state.data())
for i in range(100):
velocities = sr.JointVelocities(ds.evaluate(current_state))
current_state += timedelta(milliseconds=100) * velocities
self.assertTrue(
current_state.dist(attractor, sr.JointStateVariable.POSITIONS) <
1e-3)
def test_pose(self):
ds = CartesianPointAttractorDS()
target = sr.CartesianPose.Random("B")
ds.set_parameter(
sr.Parameter("attractor", target,
sr.StateType.PARAMETER_CARTESIANSTATE))
current_pose = sr.CartesianPose.Identity("B")
for i in range(100):
twist = sr.CartesianTwist(ds.evaluate(current_pose))
current_pose += timedelta(milliseconds=100) * twist
self.assertTrue(
current_pose.dist(target, sr.CartesianStateVariable.POSITION) <
1e-3)
self.assertTrue(
current_pose.dist(target, sr.CartesianStateVariable.ORIENTATION) <
1e-3)
def test_controller_with_robot_and_params(self):
parameters = [
sr.Parameter("damping", 5.0, sr.StateType.PARAMETER_DOUBLE)
]
robot = Model(
"robot",
os.path.join(os.path.dirname(os.path.realpath(__file__)),
"panda_arm.urdf"))
self.assertRaises(RuntimeError, create_joint_controller,
CONTROLLER_TYPE.NONE, parameters, robot)
ctrl = create_joint_controller(CONTROLLER_TYPE.IMPEDANCE, parameters,
robot)
self.assertNotEqual(ctrl, None)
self.assertEqual(
ctrl.get_parameter_value("damping").size,
robot.get_number_of_joints() * robot.get_number_of_joints())
self.assertEqual(
ctrl.get_parameter_value("damping").sum(),
5.0 * robot.get_number_of_joints())
def test_is_compatible(self):
ds = CartesianCircularDS()
state1 = sr.CartesianState.Identity("world", "A")
state2 = sr.CartesianState("D", "C")
state3 = sr.CartesianState("C", "A")
state4 = sr.CartesianState("C", "world")
with self.assertRaises(RuntimeError):
ds.evaluate(state1)
ds.set_base_frame(state1)
with self.assertRaises(RuntimeError):
ds.evaluate(state2)
with self.assertRaises(RuntimeError):
ds.evaluate(state3)
with self.assertRaises(RuntimeError):
ds.evaluate(state4)
ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.StateType.PARAMETER_ELLIPSOID))
self.assertTrue(ds.is_compatible(state1))
self.assertFalse(ds.is_compatible(state2))
self.assertTrue(ds.is_compatible(state3))
self.assertTrue(ds.is_compatible(state4))
