def test_controller_with_robot(self): robot_name = "robot" urdf_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), "panda_arm.urdf") robot = Model(robot_name, urdf_path) command_state = sr.CartesianState().Identity(robot.get_frames()[-1], robot.get_base_frame()) feedback_state = sr.CartesianState().Identity(robot.get_frames()[-1], robot.get_base_frame()) joint_state = sr.JointPositions().Zero(robot_name, robot.get_number_of_joints()) self.assertRaises(RuntimeError, create_cartesian_controller, CONTROLLER_TYPE.NONE, robot) self.assertRaises(RuntimeError, create_joint_controller, CONTROLLER_TYPE.NONE, robot) cart_ctrl = create_cartesian_controller(CONTROLLER_TYPE.IMPEDANCE, robot) self.assertNotEqual(cart_ctrl, None) cart_ctrl.compute_command(command_state, feedback_state) cart_ctrl.compute_command(command_state, feedback_state, joint_state) cart_ctrl.compute_command(command_state, feedback_state, robot.compute_jacobian(joint_state)) joint_ctrl = create_joint_controller(CONTROLLER_TYPE.IMPEDANCE, robot) self.assertNotEqual(joint_ctrl, None) joint_ctrl.compute_command(joint_state, joint_state) self.assertEqual( joint_ctrl.get_parameter_value("stiffness").size, robot.get_number_of_joints() * robot.get_number_of_joints())
def test_cartesian_controller(self): command_state = sr.CartesianState().Identity("command") feedback_state = sr.CartesianState().Identity("feedback") ctrl = create_cartesian_controller(CONTROLLER_TYPE.NONE) self.assertTrue(ctrl is None) ctrl = create_cartesian_controller(CONTROLLER_TYPE.IMPEDANCE) self.assertFalse(ctrl is None) self.expected_parameters_test(ctrl, ["damping", "stiffness", "inertia"]) ctrl.compute_command(command_state, feedback_state) ctrl = create_cartesian_controller(CONTROLLER_TYPE.VELOCITY_IMPEDANCE) self.assertFalse(ctrl is None) self.expected_parameters_test(ctrl, ["damping", "stiffness"]) ctrl.compute_command(command_state, feedback_state) ctrl = create_cartesian_controller(CONTROLLER_TYPE.DISSIPATIVE) self.assertFalse(ctrl is None) self.expected_parameters_test(ctrl, ["damping"]) ctrl.compute_command(command_state, feedback_state) ctrl = create_cartesian_controller(CONTROLLER_TYPE.DISSIPATIVE_LINEAR) self.assertFalse(ctrl is None) self.expected_parameters_test(ctrl, ["damping"]) ctrl.compute_command(command_state, feedback_state) ctrl = create_cartesian_controller(CONTROLLER_TYPE.DISSIPATIVE_ANGULAR) self.assertFalse(ctrl is None) self.expected_parameters_test(ctrl, ["damping"]) ctrl.compute_command(command_state, feedback_state) ctrl = create_cartesian_controller( CONTROLLER_TYPE.DISSIPATIVE_DECOUPLED) self.assertFalse(ctrl is None) self.expected_parameters_test(ctrl, ["damping"]) ctrl.compute_command(command_state, feedback_state) ctrl = create_cartesian_controller(CONTROLLER_TYPE.COMPLIANT_TWIST) self.assertFalse(ctrl is None) parameters = ctrl.get_parameters() expected_parameters = [ "linear_principle_damping", "linear_orthogonal_damping", "angular_stiffness", "angular_damping" ] [ self.assertTrue(key in parameters.keys()) for key in expected_parameters ] ctrl.compute_command(command_state, feedback_state) self.assertRaises(RuntimeError, ctrl.compute_command, command_state, feedback_state, sr.JointPositions().Zero("robot", 3))
def test_cartesian_impedance(self): ctrl = create_cartesian_controller(CONTROLLER_TYPE.IMPEDANCE) desired_state = sr.CartesianState("test") desired_state.set_linear_velocity([1, 0, 0]) feedback_state = sr.CartesianState("test") feedback_state.set_orientation([0, 0, 1, 1]) feedback_state.set_linear_velocity([0.5, 0, 0]) command = ctrl.compute_command(desired_state, feedback_state) self.assertTrue(np.linalg.norm(command.data()) > 0)
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_cartesian_to_joint_impedance(self): ctrl = create_cartesian_controller(CONTROLLER_TYPE.VELOCITY_IMPEDANCE) desired_state = sr.CartesianState("test") desired_state.set_linear_velocity([1, 0, 0]) feedback_state = sr.CartesianState("test") feedback_state.set_linear_velocity([0.5, 0, 0]) jac = sr.Jacobian().Random("test_robot", 3, "test") command = ctrl.compute_command(desired_state, feedback_state, jac) self.assertTrue(np.linalg.norm(command.data()) > 0) feedback_state.set_linear_velocity(desired_state.get_linear_velocity()) command = ctrl.compute_command(desired_state, feedback_state, jac) self.assertTrue(np.linalg.norm(command.data()) > 0)
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))