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_compute_task_to_joint_command(self):
ctrl = create_cartesian_controller(CONTROLLER_TYPE.DISSIPATIVE)
desired_twist = sr.CartesianTwist("test", [1, 0, 0])
feedback_twist = sr.CartesianTwist("test", [1, 1, 0])
jac = sr.Jacobian().Random("test_robot", 3, "test")
command = ctrl.compute_command(desired_twist, feedback_twist, jac)
self.assertTrue(np.linalg.norm(command.get_torques()) > 0)
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_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_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_compute_command_with_colinear_velocity(self):
tolerance = 1e-4
ctrl = create_cartesian_controller(CONTROLLER_TYPE.DISSIPATIVE_LINEAR)
eigenvalues = ctrl.get_parameter_value("damping_eigenvalues")
eigenvalues[0] = 10
ctrl.set_parameter_value("damping_eigenvalues", eigenvalues,
sr.StateType.PARAMETER_VECTOR)
desired_twist = sr.CartesianTwist("test", [1, 0, 0])
feedback_twist = sr.CartesianTwist("test", [1, 1, 0])
command = ctrl.compute_command(desired_twist,
sr.CartesianTwist.Zero("test"))
self.assertTrue(abs(command.get_force()[0] - 10) < tolerance)
self.assertTrue(abs(command.get_force()[1]) < tolerance)
self.assertTrue(abs(command.get_force()[2]) < tolerance)
command = ctrl.compute_command(desired_twist, feedback_twist)
self.assertTrue(abs(command.get_force()[0]) < tolerance)
self.assertTrue(abs(command.get_force()[1] - -1) < tolerance)
self.assertTrue(abs(command.get_force()[2]) < tolerance)
def setUpClass(cls):
cls.ctrl = create_cartesian_controller(CONTROLLER_TYPE.COMPLIANT_TWIST)
cls.command_twist = sr.CartesianTwist().Zero("test")
cls.feedback_twist = sr.CartesianTwist().Zero("test")
