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