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