def test_joint_controller(self):
dim = 3
command_state = sr.JointState().Zero("robot", dim)
feedback_state = sr.JointState().Zero("robot", dim)
ctrl = create_joint_controller(CONTROLLER_TYPE.NONE, 3)
self.assertEqual(ctrl, None)
ctrl = create_joint_controller(CONTROLLER_TYPE.IMPEDANCE, dim)
self.assertNotEqual(ctrl, None)
self.expected_parameters_test(ctrl,
["damping", "stiffness", "inertia"], dim)
ctrl.compute_command(command_state, feedback_state)
ctrl = create_joint_controller(CONTROLLER_TYPE.VELOCITY_IMPEDANCE, dim)
self.assertNotEqual(ctrl, None)
self.expected_parameters_test(ctrl, ["damping", "stiffness"], dim)
ctrl.compute_command(command_state, feedback_state)
ctrl = create_joint_controller(CONTROLLER_TYPE.DISSIPATIVE, dim)
self.assertFalse(ctrl is None)
self.expected_parameters_test(ctrl, ["damping"], dim)
self.assertRaises(RuntimeError, create_joint_controller,
CONTROLLER_TYPE.DISSIPATIVE_LINEAR)
self.assertRaises(RuntimeError, create_joint_controller,
CONTROLLER_TYPE.DISSIPATIVE_ANGULAR)
self.assertRaises(RuntimeError, create_joint_controller,
CONTROLLER_TYPE.DISSIPATIVE_DECOUPLED)
self.assertRaises(RuntimeError, create_joint_controller,
CONTROLLER_TYPE.COMPLIANT_TWIST)
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_joint_command(self):
ctrl = create_joint_controller(CONTROLLER_TYPE.DISSIPATIVE, 4)
desired_velocities = sr.JointVelocities("test", [1, 0, 0, 0])
feedback_velocities = sr.JointVelocities("test", [1, 1, 0, 0])
command = ctrl.compute_command(desired_velocities, feedback_velocities)
self.assertTrue(np.linalg.norm(command.get_torques()) > 0)
def test_joint_impedance(self):
nb_joints = 3
ctrl = create_joint_controller(CONTROLLER_TYPE.IMPEDANCE, nb_joints)
desired_state = sr.JointState("test", nb_joints)
desired_state.set_velocities([1, 0, 0])
feedback_state = sr.JointState("test", nb_joints)
feedback_state.set_positions([0, 0, 1])
feedback_state.set_velocities([0.5, 0, 0])
command = ctrl.compute_command(desired_state, feedback_state)
self.assertTrue(np.linalg.norm(command.data()) > 0)
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())