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