From b2a9228de05b590cd4501779b445a59340ffd2d4 Mon Sep 17 00:00:00 2001 From: Omid Date: Tue, 18 Jun 2024 10:00:58 -0500 Subject: [PATCH 1/2] Cartesian velocity control --- lbr_demos/lbr_demos_py/lbr_demos_py/asbr.py | 521 ++++++++++++++++++ .../lbr_demos_py/joint_sine_overlay.py | 5 + .../lbr_demos_py/lbr_demos_py/move_2_cart.py | 208 +++++++ .../lbr_demos_py/pose_planning.py | 47 ++ lbr_demos/lbr_demos_py/setup.py | 2 + lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp | 10 +- .../lbr_fri_ros2/interfaces/base_command.hpp | 1 - lbr_fri_ros2/src/filters.cpp | 28 +- lbr_fri_ros2/src/interfaces/base_command.cpp | 11 +- .../src/interfaces/position_command.cpp | 15 +- .../src/interfaces/torque_command.cpp | 3 - .../src/interfaces/wrench_command.cpp | 3 - .../config/lbr_system_parameters.yaml | 2 +- 13 files changed, 812 insertions(+), 44 deletions(-) create mode 100644 lbr_demos/lbr_demos_py/lbr_demos_py/asbr.py create mode 100644 lbr_demos/lbr_demos_py/lbr_demos_py/move_2_cart.py create mode 100644 lbr_demos/lbr_demos_py/lbr_demos_py/pose_planning.py diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/asbr.py b/lbr_demos/lbr_demos_py/lbr_demos_py/asbr.py new file mode 100644 index 00000000..e6ed1730 --- /dev/null +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/asbr.py @@ -0,0 +1,521 @@ +'''Collection of functions/types from the ASBR class taken in Spring 2022 + +Most code is derived from the Matlab code that was written during that class, +and is broadly attributable to Dr Alambeigi (who taught the course). +''' + +import numpy as np +import quaternion # adds quaternion type to numpy namespace +from math import sin, cos, atan2, asin, pi +from scipy import linalg +import geometry_msgs.msg # For conversion to/from message types + +# numpy also offers versions of these +def deg2rad(d): + if isinstance(d, np.ndarray): + return np.deg2rad(d) + elif isinstance(d, list): + return [di / 180 * pi for di in d] + elif isinstance(d, tuple): + return tuple([di / 180 * pi for di in d]) + else: + return d / 180 * pi +def rad2deg(r): + if isinstance(r, np.ndarray): + return np.rad2deg(r) + elif isinstance(r, list): + return [ri / pi * 180 for ri in r] + elif isinstance(r, tuple): + return tuple([ri / pi * 180 for ri in r]) + else: + return r / pi * 180 + +def is_near(a, b, close_enough=1e-14): + return abs(a - b) < close_enough + +def is_eye(a, close_enough=1e-14): + return np.allclose(a, np.eye(a.shape[0]), atol=close_enough) + +def skewsym(v): + if v.size == 3: + return np.array([[ 0, -v[2], v[1]], + [ v[2], 0, -v[0]], + [-v[1], v[0], 0]]) + elif v.size == 6: + return np.array([[ 0, -v[2], v[1], v[3]], + [ v[2], 0, -v[0], v[4]], + [-v[1], v[0], 0, v[5]], + [ 0, 0, 0, 0]]) + else: + raise Exception('Not implemented for this shape of input: ' + str(v.size)) + + + +class Degrees(): + def __init__(self, x_): + self.x = x_ + def __str__(self): + return str(self.x) + @property + def deg(self): + return self.x + @deg.setter + def deg(self, val): + self.x = val + @property + def rad(self): + return deg2rad(self.x) + @rad.setter + def rad(self, val): + self.x = rad2deg(val) + +# Discovered Rotation is in scipy.spatial.transform. +#from scipy.spatial.transform import Rotation +# But nothing else is... so we still have to write a bunch of stuff. Maybe just redo scipy work. +class Rotation(): + def __init__(self, init_value=None): + if init_value is None: + self.m = np.eye(3) + elif isinstance(init_value, np.quaternion): + self.m = quaternion.as_rotation_matrix(init_value) + elif isinstance(init_value, geometry_msgs.msg.Quaternion): + self.m = quaternion.as_rotation_matrix(np.quaternion(init_value.w, init_value.x, init_value.y, init_value.z)) + elif isinstance(init_value, np.ndarray): + assert(len(init_value.shape) == 2) + assert(init_value.shape[0] == 3) + assert(init_value.shape[1] == 3) + self.m = init_value + elif isinstance(init_value, Rotation): + self.m = np.copy(init_value.m) + else: + raise Exception('Dont know how to init Rotation from this type') + def __str__(self): + return str(self.m) + def __mul__(self, other): + return Rotation(np.matmul(self.m, other.m)) + @staticmethod + def eye(): + return Rotation(None) + @staticmethod + def from_zyx(euler, degrees=False): + if degrees: + eulerrad = deg2rad(euler) + else: + eulerrad = euler + # Unfortunately, quaternion module doesn't support various euler conventions + # So I implement via ASBR's rpy2rot function from Matlab. + alpha = eulerrad[0]; beta = eulerrad[1]; gamma = eulerrad[2] # reverses, bc formula is rpy + ca = cos(alpha); sa = sin(alpha) + cb = cos(beta); sb = sin(beta) + cg = cos(gamma); sg = sin(gamma) + return Rotation(np.array([[ca*cb, ca*sb*sg-sa*cg, ca*sb*cg+sa*sg], + [sa*cb, sa*sb*sg+ca*cg, sa*sb*cg-ca*sg], + [-sb, cb*sg, cb*cg]])) + @staticmethod + def from_zyx_degrees(euler): + # Just forwarding, for backward compatibility. + return Rotation.from_zyx(euler, degrees=True) + @staticmethod + def from_ABC(abc, degrees=False): + # An alias for zyx + return Rotation.from_zyx(abc, degrees=degrees) + # Alternative implementation via quaternions: + # https://doc.rc-visard.com/v21.07/en/pose_format_kuka.html + #if degrees: + # (Ar, Br, Cr) = deg2rad(abc) + #else: + # (Ar, Br, Cr) = abc + #x = cos(Ar/2) * cos(Br/2) * sin(Cr/2) - sin(Ar/2) * sin(Br/2) * cos(Cr/2) + #y = cos(Ar/2) * sin(Br/2) * cos(Cr/2) + sin(Ar/2) * cos(Br/2) * sin(Cr/2) + #z = sin(Ar/2) * cos(Br/2) * cos(Cr/2) - cos(Ar/2) * sin(Br/2) * sin(Cr/2) + #w = cos(Ar/2) * cos(Br/2) * cos(Cr/2) + sin(Ar/2) * sin(Br/2) * sin(Cr/2) + #return Rotation(np.quaternion(w, x, y, z)) + @staticmethod + def from_ABC_degrees(abc_deg): + # Just forwarding, for backward compatibility. + return Rotation.from_ABC(abc_deg, degrees=True) + @staticmethod + def from_rpy(rpy, degrees=False): + return Rotation.from_zyx(np.flip(rpy), degrees=degrees) + def is_eye(self): + return is_eye(self.m) + def as_zyx(self, degrees=False): + # TODO: what is the range of these? I think pitch is -pi/2 to +pi/2. Others are... not sure. + if self.is_eye(): + return np.zeros([3, 1]) + else: + alpha = atan2(self.m[1,0], self.m[0,0]) + beta = atan2(-self.m[2,0], np.sqrt(self.m[2,1]**2 + self.m[2,2]**2)) + gamma = atan2(self.m[2,1], self.m[2,2]) + ans = np.array([alpha, beta, gamma]) + return np.rad2deg(ans) if degrees else ans + def as_ABC(self, degrees=False): + # Just an alias for zyx + return self.as_zyx(degrees=degrees) + # Alternative implementation, seems to be the same, but worse numerical stability (?), from: + # https://doc.rc-visard.com/v21.07/en/pose_format_kuka.html + #q = self.as_quat() # The formulas I have use a quaternion, so need that first. + #A = atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y**2 + q.z**2)) + #B = asin(2 * (q.w * q.y - q.z * q.x)) + #C = atan2(2 * (q.w * q.x + q.y * q.z), 1 - 2 * (q.x**2 + q.y**2)) + #ans = np.array([A, B, C]).T + #return np.rad2deg(ans) if degrees else ans + def as_rpy(self, degrees=False): + # Same as ZYX, but presented in different order. + zyx = self.as_zyx(degrees) + return np.flip(zyx) + def as_quat(self): + return quaternion.from_rotation_matrix(self.m) + def as_geometry_orientation(self): + q = self.as_quat() + return geometry_msgs.msg.Quaternion(x=q.x, y=q.y, z=q.z, w=q.w) + +class Translation(): + def __init__(self, init_xyz=None): + if init_xyz is not None: + if isinstance(init_xyz, geometry_msgs.msg.Point): + self.m = np.array([init_xyz.x, init_xyz.y, init_xyz.z]) + elif isinstance(init_xyz, geometry_msgs.msg.Vector3): + self.m = np.array([init_xyz.x, init_xyz.y, init_xyz.z]) + elif isinstance(init_xyz, HPoint): + self.m = init_xyz.xyz + elif isinstance(init_xyz, Translation): + self.m = np.copy(init_xyz.m) + else: + self.m = np.squeeze(np.array(init_xyz)) + assert(self.m.size == 3) + else: + self.m = np.zeros([3,1]) + def __str__(self): + return str(self.m) + def __add__(self, other): + return Translation(self.m + other.m) + @property + def x(self): + return self.m[0] + @x.setter + def x(self, val): + self.m[0] = val + @property + def y(self): + return self.m[1] + @y.setter + def y(self, val): + self.m[1] = val + @property + def z(self): + return self.m[2] + @z.setter + def z(self, val): + self.m[2] = val + def distance(self): + return np.linalg.norm(self.m) + def scale(self, factor): + self.m = self.m * factor + def convert_m_to_mm(self): + self.scale(1000.) + def convert_mm_to_m(self): + self.scale(0.001) + def scaled(self, factor): + return Translation(self.m * factor) + def scaled_m_to_mm(self): + return self.scaled(1000.) + def scaled_mm_to_m(self): + return self.scaled(0.001) + def as_geometry_point(self): + return geometry_msgs.msg.Point(x=self.x, y=self.y, z=self.z) + +class Transformation: + def __init__(self, *args): + if len(args) == 1: + init_value = args[0] + if isinstance(init_value, np.ndarray): + assert(len(init_value.shape) == 2) + assert(init_value.shape[0] == 4) + assert(init_value.shape[1] == 4) + assert(np.allclose(init_value[3, :], [0, 0, 0, 1])) + self.m = init_value + elif isinstance(init_value, geometry_msgs.msg.Pose): + self.m = np.eye(4) + self.rotation = Rotation(init_value.orientation) + self.translation = Translation(init_value.position) + elif isinstance(args[0], Translation): + self.m = np.eye(4) + self.translation = args[0] + else: + raise Exception('Dont know how to init Transformation from this init_value type') + elif len(args) == 2: + if isinstance(args[0], Translation) and isinstance(args[1], Rotation): + self.m = np.eye(4) + self.translation = args[0] + self.rotation = args[1] + else: + raise Exception('Dont know how to init Transformation from these args of type {} and {}'.format(type(args[0]), type(args[1]))) + else: + self.m = np.eye(4) + def __str__(self): + return str(self.m) + def __mul__(self, other): + if isinstance(other, Transformation): + return Transformation(self.m @ other.m) + else: + raise TypeError('Invalid type of multiplication argument for Transformation: ' + str(type(other))) + def rot(self): + assert(np.allclose(self.m[3, :], [0, 0, 0, 1])) + #return self.m[0:3, 0:3] + #return Rotation.from_matrix(self.m[0:3, 0:3]) + return Rotation(self.m[0:3, 0:3]) + @property + def rotation(self): + return Rotation(self.m[0:3, 0:3]) + @rotation.setter + def rotation(self, val): + if isinstance(val, Rotation): + self.m[0:3, 0:3] = val.m + elif isinstance(val, np.ndarray): + assert(len(val.shape) == 2) + assert(val.shape[0] == 3) + assert(val.shape[1] == 3) + self.m[0:3, 0:3] = val + else: + raise Exception('dont know how to set rotation') + @property + def translation(self): + return Translation(self.m[0:3, 3]) + @translation.setter + def translation(self, val): + if isinstance(val, Translation): + self.m[0:3, 3] = np.squeeze(val.m) + elif isinstance(val, np.ndarray): + #assert(len(val.shape) == 2) + #assert(val.shape[0] == 3) + #assert(val.shape[1] == 3) + assert(val.size == 3) + self.m[0:3, 3] = val + else: + raise Exception('dont know how to set translation') + @property + def matrix(self): + return self.m + @property + def inv(self): + return Transformation(np.linalg.inv(self.m)) + def convert_m_to_mm(self): + # Converts in place. Assumes translation is currently in m, and makes it in mm. + self.m[0:3, 3] = self.m[0:3, 3] * 1000. + def convert_mm_to_m(self): + # Converts in place. Assumes translation is currently in mm, and makes it in m. + self.m[0:3, 3] = self.m[0:3, 3] / 1000. + def scaled(self, factor): + return Transformation(self.translation.scaled(factor), self.rotation) + def scaled_m_to_mm(self): + # Assumes translation is currently in m, and makes it in mm. + return Transformation(self.translation.scaled_m_to_mm(), self.rotation) + def scaled_mm_to_m(self): + # Assumes translation is currently in mm, and makes it in m. + return Transformation(self.translation.scaled_mm_to_m(), self.rotation) + def transform_hpoint(self, p): + # Premultiplies the given homogenous point (HPoint) by this transform. + # Returns the transformed homogenous point. + assert(isinstance(p, HPoint)) + return HPoint(np.matmul(self.m, p.m)) + def as_geometry_pose(self): + return geometry_msgs.msg.Pose(position=self.translation.as_geometry_point(), + orientation=self.rotation.as_geometry_orientation()) + @staticmethod + def from_xyz_mm_ABC_degrees(xyzABC): + transl = Translation(xyzABC[0:3]) + rot = Rotation.from_ABC_degrees(xyzABC[3:6]) + return Transformation(transl, rot) + @staticmethod + def compose(A, B): + return Transformation(np.matmul(A.matrix, B.matrix)) + +class HPoint(): + # Homogenous representation of a point + def __init__(self, *args): + if len(args) == 3: + self.m = np.reshape(np.array([args[0], args[1], args[2], 1.]), (4,1)) + elif len(args) == 1: + if isinstance(args[0], np.ndarray) and args[0].size == 3: + self.m = np.reshape(np.array([args[0][0], args[0][1], args[0][2], 1.]), (4,1)) + elif isinstance(args[0], np.ndarray) and args[0].size == 4: + self.m = np.reshape(args[0], (4,1)) + else: + raise Exception('Dont know how to init') + else: + raise Exception('Dont know how to init') + def __str__(self): + return str(self.m) + @property + def x(self): + return self.m[0,0] + @x.setter + def x(self, val): + self.m[0,0] = val + @property + def y(self): + return self.m[1,0] + @y.setter + def y(self, val): + self.m[1,0] = val + @property + def z(self): + return self.m[2,0] + @z.setter + def z(self, val): + self.m[2,0] = val + @property + def xyz(self): + return self.m[0:3,0] + +class Robot(object): + def __init__(self): + pass + def __str__(self): + return self.name + @staticmethod + def create_iiwa(): + r = Robot() + r.name = 'Kuka iiwa R14' + # End effector transofrmation matrix in straight-up home position (mm) + r.home = np.array([[1., 0., 0., 0.], + [0., 1., 0., 0.], + [0., 0., 1., 1306.], + [0., 0., 0., 1.]]) + # Directions of axes in straight-up home position + r.axes = np.array([[0, 0, 1], + [0, 1, 0], + [0, 0, 1], + [0, -1, 0], + [0, 0, 1], + [0, 1, 0], + [0, 0, 1]]) + # Location of each joint center, in straight-up home position (mm) + r.offset = np.array([[0, 0, 170], + [0, 0, 360], + [0, 0, 600], + [0, 0, 780], + [0, 0, 1000], + [0, -60, 1180], + [0, 0, 1271]]) + # DOF from arrays we defined already + r.dof = r.axes.shape[0] + # Calculate screw vectors + # Each axis is a column, so the result is 6 x dof. + r.screw = np.empty([6, r.dof]) + for i in range(r.dof): + r.screw[0:3, i] = r.axes[i, :].transpose() + r.screw[3:6, i] = -np.cross(r.axes[i, :].transpose(), r.offset[i, :].transpose()) + # Return the resulting struct + return r + def FK_space(self, joint_angles): + """Returns transformation matrix for end effector of the given robot at the given + joint angles. + + joint_angles: list/vector of joints in radians + + Returns 4x4 transformation matrix of end effector""" + cum_t = np.eye(4) + for i in range(self.dof): + skew_s = skewsym(self.screw[:, i]) + cum_t = np.matmul(cum_t, linalg.expm(skew_s * joint_angles[i])) + return Transformation(np.matmul(cum_t, self.home)) + + + + + +def _unit_test_FK_space(): + r = Robot.create_iiwa() + # Matlab example 4, ans x/y/z/A/B/C = -636, -158, 1012, 68, -43, -35 + ja_deg = [-0.01, -35.10, 47.58, 24.17, 0.00, 0.00, 0.00] + ja_rad = np.deg2rad(ja_deg) + t = r.FK_space(ja_rad) + assert(np.allclose(t.rotation.as_zyx(degrees=True), [68.28584782, -43.53993415, -35.84364870], atol=1e-8)) + assert(np.allclose(t.rotation.as_rpy(degrees=True), [-35.844, -43.540, 68.286], atol=1e-8)) + assert(np.allclose(t.translation.m, [-636.32792290, -158.87809407, 1012.70702237], atol=1e-8)) + +def _unit_test_Rotation_from_ABC_vs_legacy(): + '''Compare to a legacy standalone function. Which prob doesn't exist any longer.''' + def ABCdeg_to_quat(adeg, bdeg, cdeg): + '''Legacy function, from https://doc.rc-visard.com/v21.07/en/pose_format_kuka.html''' + (Ar, Br, Cr) = (deg2rad(d) for d in (adeg, bdeg, cdeg)) + x = cos(Ar/2) * cos(Br/2) * sin(Cr/2) - sin(Ar/2) * sin(Br/2) * cos(Cr/2) + y = cos(Ar/2) * sin(Br/2) * cos(Cr/2) + sin(Ar/2) * cos(Br/2) * sin(Cr/2) + z = sin(Ar/2) * cos(Br/2) * cos(Cr/2) - cos(Ar/2) * sin(Br/2) * sin(Cr/2) + w = cos(Ar/2) * cos(Br/2) * cos(Cr/2) + sin(Ar/2) * sin(Br/2) * sin(Cr/2) + return (x, y, z, w) + q = Rotation.from_ABC_degrees([20,30,-40]).as_quat() + qxyzw = (q.x, q.y, q.z, q.w) + q2 = ABCdeg_to_quat(20,30,-40) + assert(np.allclose(qxyzw, q2, atol=1e-12)) + q = Rotation.from_ABC_degrees([-15,22,10]).as_quat() + qxyzw = (q.x, q.y, q.z, q.w) + q2 = ABCdeg_to_quat(-15,22,10) + assert(np.allclose(qxyzw, q2, atol=1e-12)) + q = Rotation.from_ABC_degrees([0,190,-600]).as_quat() + qxyzw = (q.x, q.y, q.z, q.w) + q2 = ABCdeg_to_quat(0,190,-600) + assert(np.allclose(qxyzw, q2, atol=1e-12)) + +def _unit_test_Rotation_from_zyx(): + assert(np.allclose(Rotation.from_zyx([0, 0, 0]).m, np.array([[1,0,0],[0,1,0],[0,0,1]]))) + assert(np.allclose(quaternion.as_float_array(Rotation.from_zyx([0, 0, 0]).as_quat()), np.array([1, 0, 0, 0]), atol=1e-12)) + + assert(quaternion.isclose(Rotation.from_zyx([pi * 1.001, 0, pi * 1.001]).as_quat(), np.quaternion(0.000002467399071, -0.001570793742940, 0.999997532600929, -0.001570793742940))) + assert(quaternion.isclose(Rotation.from_zyx([pi * 0.999, 0, pi * 0.999]).as_quat(), np.quaternion(0.000002467399071, 0.001570793742940, 0.999997532600929, 0.001570793742940))) + assert(quaternion.isclose(Rotation.from_zyx([pi, 0, pi]).as_quat(), np.quaternion(0.0, 0.0, 1.0, 0.0))) + print(Rotation.from_zyx([pi/4, 0, pi]).as_quat(), np.quaternion(0.0, 0.923879532511287, 0.382683432365090, 0.000000005268356)) + print(Rotation.from_ABC([pi/4, 0, pi]).as_quat(), np.quaternion(0.0, 0.923879532511287, 0.382683432365090, 0.000000005268356)) + assert(quaternion.isclose(Rotation.from_zyx([pi/4, 0, pi]).as_quat(), np.quaternion(0.0, 0.923879532511287, 0.382683432365090, 0.000000005268356), atol=1e-3)) + +def _unit_test_Rotation_zyx_is_ABC(verbose=False): + egs = [[1, 0, 0], [0, 1, 0], [0, 0, 1], [pi/2, -pi/4, 0]] + if verbose: print('from_ABC -> as_ABC') + for eg in egs: + ans = Rotation.from_ABC(eg).as_ABC() + if verbose: print(eg, ans, ans - eg) + assert(np.allclose(ans - eg, 0, atol=1e-10)) + if verbose: print('from_ABC -> as_zyx') + for eg in egs: + ans = Rotation.from_ABC(eg).as_zyx() + if verbose: print(eg, ans, ans - eg) + assert(np.allclose(ans - eg, 0, atol=1e-10)) + if verbose: print('from_zyx -> as_zyx') + for eg in egs: + ans = Rotation.from_zyx(eg).as_zyx() + if verbose: print(eg, ans, ans - eg) + assert(np.allclose(ans - eg, 0, atol=1e-10)) + if verbose: print('from_zyx -> as_ABC') + for eg in egs: + ans = Rotation.from_zyx(eg).as_ABC() + if verbose: print(eg, ans, ans - eg) + assert(np.allclose(ans - eg, 0, atol=1e-10)) + if verbose: print('Rotation matrices from_ABC followed by from zyx') + for eg in egs: + anszyx = Rotation.from_zyx(eg) + ansabc = Rotation.from_ABC(eg) + if verbose: print(ansabc), print(anszyx) + assert(np.allclose(anszyx.m, ansabc.m, atol=1e-10)) + +def _unit_test_Rotation_rpy_is_zyx_reversed(verbose=False): + egs = [[1, 0, 0], [0, 1, 0], [0, 0, 1], [pi/2, -pi/4, 0]] + for eg in egs: + anszyx = Rotation.from_zyx(eg) + ansrpy = Rotation.from_rpy(np.flip(eg)) + if verbose: print(ansabc), print(ansrpy) + assert(np.allclose(anszyx.m, ansrpy.m, atol=1e-10)) + for eg in egs: + rpy = Rotation.from_zyx(eg).as_rpy() + assert(np.allclose(np.flip(rpy), eg, atol=1e-10)) + +def _unit_test(): + _unit_test_FK_space() + _unit_test_Rotation_from_ABC_vs_legacy() + #_unit_test_Rotation_from_zyx() + _unit_test_Rotation_zyx_is_ABC() + _unit_test_Rotation_rpy_is_zyx_reversed() + +if __name__ == '__main__': + _unit_test() \ No newline at end of file diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py index 0cc373d3..ebb61fd0 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py @@ -49,6 +49,10 @@ def _on_lbr_state(self, lbr_state: LBRState) -> None: self._lbr_joint_position_command.joint_position[ 3 ] += self._amplitude * math.sin(self._phase) + + self._lbr_joint_position_command.joint_position[ + 5 + ] += -self._amplitude * math.sin(self._phase) self._phase += 2 * math.pi * self._frequency * self._dt self._lbr_joint_position_command_pub.publish( @@ -56,6 +60,7 @@ def _on_lbr_state(self, lbr_state: LBRState) -> None: ) else: # reset phase + print('resetting') self._phase = 0.0 def _retrieve_update_rate(self) -> float: diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/move_2_cart.py b/lbr_demos/lbr_demos_py/lbr_demos_py/move_2_cart.py new file mode 100644 index 00000000..bdd23b21 --- /dev/null +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/move_2_cart.py @@ -0,0 +1,208 @@ +#! /usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Pose +import math +import copy +import time +import numpy as np +from lbr_demos_py.asbr import * + +class Move2Cart(Node): + + def __init__(self): + super().__init__('move_2_cart') + self.is_init = False + self.curr_pose = Pose() + self.goal_pose = Pose() # get from constructor? + + self.communication_rate = 0.01 # 10 ms + self.pid_p_correction = 12.2 # For compensating delay during execution. This value -in accordance with p = 12.2 - was found to be good by manual testings. + # See: https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/187 + + self.move_orientation_init_time = -1 + + self.pose_pub = self.create_publisher(Pose, 'command/pose', 1) + self.pose_sub = self.create_subscription(Pose, 'state/pose', self.on_pose, 1) + self.flag=1 + + + def on_pose(self, msg): + + if not self.is_init: + self.is_init = True + self.initial_pose = msg + + #Specify goal pose + + self.goal_pose.position.x = 0.5 + self.goal_pose.position.y = 0.0 + self.goal_pose.position.z = 0.55 + + goal_orientation = Rotation(self.initial_pose.orientation) + goal_orientation = goal_orientation.as_ABC(True) + goal_orientation[1]=10.0 + goal_orientation[2]=-160.0 + goal_orientation[0]=180.0 + goal_orientation = Rotation.from_ABC(goal_orientation,True) + goal_orientation = goal_orientation.as_geometry_orientation() + self.goal_pose.orientation = goal_orientation + + self.desired_pose = msg # desired_pose: the pose which the robot should be in, at the moment. This is useful particularly in orientation control. + # For some reason, the orientation gets some noise during execution. Therefore, relying on curr_pose.orientation for generating + # commands was giving problems. + + print('starting at:', int(round(time.time() * 1000))) # Current time in milliseconds + + + else: + + self.curr_pose = msg + + if (self.is_close_pos() and self.is_close_orien()): + + if(self.flag==1): + current_time_ms = int(round(time.time() * 1000)) # Current time in milliseconds + print(f'Time (ms): {current_time_ms}') + self.flag=0 + return + elif (self.is_close_pos()): + MotionTime = 20.0 #s + command_pose = self.generate_move_command_rotation(MotionTime) + # print('here') + else: + lin_vel = 0.01 # replace the number with desired linear velocity in m/s + command_pose = self.generate_move_command(lin_vel) + # print('I AM HERE!') + + + if (command_pose.position.x > 0.75 or command_pose.position.x < 0.35 or + command_pose.position.y > 0.1 or command_pose.position.y < -0.1 or + command_pose.position.z > 0.78 or command_pose.position.z < 0.5): #Command guarding + + print('Failed, not executable') + + else: + self.pose_pub.publish(command_pose) + + + def generate_move_command(self, lin_vel, pos_thresh = 0.0005): #Generates move commands, for motions containing ONLY rotational movements use generate_move_command_rotation method + command_pose = Pose() + GoalPose = self.goal_pose + CurrPose = self.curr_pose + + translation_vec = np.asarray([GoalPose.position.x - CurrPose.position.x, + GoalPose.position.y - CurrPose.position.y, + GoalPose.position.z - CurrPose.position.z]) + if(np.linalg.norm(translation_vec)<2*pos_thresh): + vel_vec = (translation_vec / np.linalg.norm(translation_vec)) * lin_vel + else: + vel_vec = (translation_vec / np.linalg.norm(translation_vec)) * lin_vel * self.pid_p_correction + command_pose.position.x = CurrPose.position.x + vel_vec[0] * self.communication_rate + command_pose.position.y = CurrPose.position.y + vel_vec[1] * self.communication_rate + command_pose.position.z = CurrPose.position.z + vel_vec[2] * self.communication_rate + + + timeRemain2Reach = np.linalg.norm(translation_vec) / lin_vel + if(int(timeRemain2Reach/self.communication_rate)>0): + curr_Rot = Rotation(CurrPose.orientation) + goal_Rot = Rotation(GoalPose.orientation) + ABC_diff = goal_Rot.as_ABC() - (Rotation(self.desired_pose.orientation)).as_ABC() + for i in range(3): + if(ABC_diff[i]>np.pi): + ABC_diff[i]-=2.0*np.pi + elif(ABC_diff[i]<(-np.pi)): + ABC_diff[i]+=2.0*np.pi + + ABC_step = (ABC_diff / timeRemain2Reach) * self.communication_rate + ABC_command = ABC_step + (Rotation(self.desired_pose.orientation)).as_ABC() + quat_command = Rotation.from_ABC(ABC_command) + + command_pose.orientation = quat_command.as_geometry_orientation() + self.desired_pose = copy.deepcopy(command_pose) + self.last_command = command_pose + else: + command_pose.orientation = self.last_command.orientation + # command_pose.orientation = self.initial_pose.orientation + + + return command_pose + + def generate_move_command_rotation(self, motion_time, angle_thresh = 0.1*3.1415/180.0): + GoalPose = self.goal_pose + CurrPose = self.curr_pose + command_pose = copy.deepcopy(CurrPose) + + if(self.move_orientation_init_time == -1): + self.move_orientation_init_time = int(round(time.time() * 1000))/1000.0 # Current time in seconds + else: + curr_time = int(round(time.time() * 1000))/1000.0 # Current time in seconds + timeRemain2Reach = motion_time - (curr_time - self.move_orientation_init_time) + if(int(timeRemain2Reach/self.communication_rate)>0): + curr_Rot = Rotation(CurrPose.orientation) + goal_Rot = Rotation(GoalPose.orientation) + ABC_diff = goal_Rot.as_ABC() - (Rotation(self.desired_pose.orientation)).as_ABC() + for i in range(3): + if(ABC_diff[i]>np.pi): + ABC_diff[i]-=2.0*np.pi + elif(ABC_diff[i]<(-np.pi)): + ABC_diff[i]+=2.0*np.pi + + ABC_step = (ABC_diff / timeRemain2Reach) * self.communication_rate + ABC_command = ABC_step + (Rotation(self.desired_pose.orientation)).as_ABC() + quat_command = Rotation.from_ABC(ABC_command) + + command_pose.orientation = quat_command.as_geometry_orientation() + + + # if(np.max(np.abs(ABC_command - curr_Rot.as_ABC()))>0.2*3.14159/180.0): + # if(int(round(time.time() * 1000))%5==0): + # print(ABC_command) + # print(curr_Rot.as_ABC()) + # print((Rotation(self.desired_pose.orientation)).as_ABC()) + # print('Too much jump') + # return False + + self.last_command = command_pose + self.desired_pose = copy.deepcopy(command_pose) + + else: + command_pose.orientation = self.last_command.orientation + + + return command_pose + + + + def is_close_pos(self, pos_thresh = 0.0005): + translation_vec = np.asarray([self.goal_pose.position.x - self.curr_pose.position.x, + self.goal_pose.position.y - self.curr_pose.position.y, + self.goal_pose.position.z - self.curr_pose.position.z]) + + return np.linalg.norm(translation_vec)np.pi): + ABC_diff[i]-=2.0*np.pi + elif(ABC_diff[i]<(-np.pi)): + ABC_diff[i]+=2.0*np.pi + if(np.max(np.abs(ABC_diff)); public: - JointPIDArray() = default; + JointPIDArray() = delete; + JointPIDArray(const PIDParameters &pid_parameters); void compute(const value_array_t &command_target, const value_array_t &state, const std::chrono::nanoseconds &dt, value_array_t &command); void compute(const value_array_t &command_target, const double *state, const std::chrono::nanoseconds &dt, value_array_t &command); - void initialize(const PIDParameters &pid_parameters, const double &dt); - inline const bool &is_initialized() const { return initialized_; }; - void log_info() const; protected: - bool initialized_{false}; /**< True if initialized.*/ - pid_array_t pid_controllers_; /**< PID controllers for each joint.*/ + PIDParameters pid_parameters_; /**< PID parameters for all joints.*/ + pid_array_t pid_controllers_; /**< PID controllers for each joint.*/ }; } // end of namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__FILTERS_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp index d5f61579..93c061dc 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp @@ -49,7 +49,6 @@ class BaseCommandInterface { protected: std::unique_ptr command_guard_; - PIDParameters pid_parameters_; JointPIDArray joint_position_pid_; idl_command_t command_, command_target_; }; diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp index d900876b..f2e98052 100644 --- a/lbr_fri_ros2/src/filters.cpp +++ b/lbr_fri_ros2/src/filters.cpp @@ -43,6 +43,17 @@ void JointExponentialFilterArray::initialize(const double &cutoff_frequency, initialized_ = true; } +JointPIDArray::JointPIDArray(const PIDParameters &pid_parameters) + : pid_parameters_(pid_parameters) // keep local copy of parameters since + // controller_toolbox::Pid::getGains is not const correct + // (i.e. can't be called in this->log_info) +{ + std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) { + pid.initPid(pid_parameters_.p, pid_parameters_.i, pid_parameters_.d, pid_parameters_.i_max, + pid_parameters_.i_min, pid_parameters_.antiwindup); + }); +} + void JointPIDArray::compute(const value_array_t &command_target, const value_array_t &state, const std::chrono::nanoseconds &dt, value_array_t &command) { std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable { @@ -59,11 +70,14 @@ void JointPIDArray::compute(const value_array_t &command_target, const double *s }); } -void JointPIDArray::initialize(const PIDParameters &pid_parameters, const double &dt) { - std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) { - pid.initPid(pid_parameters.p * dt, pid_parameters.i * dt, pid_parameters.d * dt, - pid_parameters.i_max * dt, pid_parameters.i_min * dt, pid_parameters.antiwindup); - }); - initialized_ = true; -} +void JointPIDArray::log_info() const { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* p: %.1f", pid_parameters_.p); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i: %.1f", pid_parameters_.i); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* d: %.1f", pid_parameters_.d); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_max: %.1f", pid_parameters_.i_max); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_min: %.1f", pid_parameters_.i_min); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* antiwindup: %s", + pid_parameters_.antiwindup ? "true" : "false"); +}; } // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/base_command.cpp b/lbr_fri_ros2/src/interfaces/base_command.cpp index c4cc5ea9..f53bddeb 100644 --- a/lbr_fri_ros2/src/interfaces/base_command.cpp +++ b/lbr_fri_ros2/src/interfaces/base_command.cpp @@ -5,7 +5,7 @@ namespace lbr_fri_ros2 { BaseCommandInterface::BaseCommandInterface(const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : pid_parameters_(pid_parameters) { + : joint_position_pid_(pid_parameters) { command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant); }; @@ -18,13 +18,6 @@ void BaseCommandInterface::init_command(const_idl_state_t_ref state) { void BaseCommandInterface::log_info() const { command_guard_->log_info(); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*** Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.p: %.1f", pid_parameters_.p); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.i: %.1f", pid_parameters_.i); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.d: %.1f", pid_parameters_.d); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.i_max: %.1f", pid_parameters_.i_max); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.i_min: %.1f", pid_parameters_.i_min); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.antiwindup: %s", - pid_parameters_.antiwindup ? "true" : "false"); + joint_position_pid_.log_info(); } } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index 73791832..26287bdf 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -19,17 +19,7 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command throw std::runtime_error(err); } #endif -#if FRICLIENT_VERSION_MAJOR == 2 - if (state.client_command_mode != KUKA::FRI::EClientCommandMode::JOINT_POSITION) { - std::string err = - "Expected robot in " + - EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::JOINT_POSITION) + - " command mode."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME()), err.c_str()); - throw std::runtime_error(err); - } -#endif -#if FRICLIENT_VERSION_MAJOR == 3 +#if FRICLIENT_VERSION_MAJOR >= 2 if (state.client_command_mode != KUKA::FRI::EClientCommandMode::JOINT_POSITION) { std::string err = "Expected robot in " + @@ -51,9 +41,6 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command } // PID - if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(pid_parameters_, state.sample_time); - } joint_position_pid_.compute( command_target_.joint_position, state.measured_joint_position, std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index 5228a443..f731fc9b 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -31,9 +31,6 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, } // PID - if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(pid_parameters_, state.sample_time); - } joint_position_pid_.compute( command_target_.joint_position, state.measured_joint_position, std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp index ee34c25b..9ab34524 100644 --- a/lbr_fri_ros2/src/interfaces/wrench_command.cpp +++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp @@ -29,9 +29,6 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, } // PID - if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(pid_parameters_, state.sample_time); - } joint_position_pid_.compute( command_target_.joint_position, state.measured_joint_position, std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), diff --git a/lbr_ros2_control/config/lbr_system_parameters.yaml b/lbr_ros2_control/config/lbr_system_parameters.yaml index ff044a74..74bf139b 100644 --- a/lbr_ros2_control/config/lbr_system_parameters.yaml +++ b/lbr_ros2_control/config/lbr_system_parameters.yaml @@ -4,7 +4,7 @@ hardware: port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection rt_prio: 80 # real-time priority for the control loop - pid_p: 10.0 # P gain for the joint position (useful for asynchronous control) + pid_p: 0.1 # P gain for the joint position (useful for asynchronous control) pid_i: 0.0 # I gain for the joint position command pid_d: 0.0 # D gain for the joint position command pid_i_max: 0.0 # max integral value for the joint position command From 28e91068b6789f5ff653ae81bd4efc94f2b2b8a5 Mon Sep 17 00:00:00 2001 From: Omid Date: Tue, 18 Jun 2024 18:51:13 -0500 Subject: [PATCH 2/2] creating a motion service --- .../lbr_demos_py/lbr_demos_py/long_lines.py | 226 ++++++++++++++++++ .../lbr_demos_py/lbr_demos_py/move_2_cart.py | 126 ++++------ .../lbr_demos_py/move_to_pose_client.py | 47 ++++ lbr_demos/lbr_demos_py/package.xml | 5 +- lbr_demos/lbr_demos_py/setup.cfg | 19 ++ lbr_demos/lbr_demos_py/setup.py | 1 + 6 files changed, 350 insertions(+), 74 deletions(-) create mode 100644 lbr_demos/lbr_demos_py/lbr_demos_py/long_lines.py create mode 100644 lbr_demos/lbr_demos_py/lbr_demos_py/move_to_pose_client.py diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/long_lines.py b/lbr_demos/lbr_demos_py/lbr_demos_py/long_lines.py new file mode 100644 index 00000000..d701f40c --- /dev/null +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/long_lines.py @@ -0,0 +1,226 @@ +import rclpy +from rclpy.node import Node +from std_msgs.msg import String, Bool, Float32 +from geometry_msgs.msg import Pose +import datetime +import numpy as np +import threading +from lbr_demos_py.asbr import * +from time import sleep + + + +first_timestamp = None + +def listen_for_enter(): + global entered + input("Press Enter to record time stamp:") + entered = True + + +def create_relative_timestamp(): + global first_timestamp + + current_time = datetime.datetime.now() + + if first_timestamp is None: + first_timestamp = current_time + + time_difference = (current_time - first_timestamp).total_seconds() * 1000 + + return time_difference + + + +class PrintLines(Node): + + def __init__(self): + super().__init__('long_lines_node') + self.goalReached = False + self.curr_pose = Pose() + self.goal_pub = self.create_publisher(Pose, 'command/Goal_Pose', 1) + self.printer_pub = self.create_publisher(Float32, 'syringeVel', 1) + self.reach_sub = self.create_subscription(Bool, 'state/Goal_Reached', self.goalReached_callback, 1) + self.pose_sub = self.create_subscription(Pose, 'state/pose', self.update_curr_pose, 1) + + # Use an event to signal the spin thread to stop + self.spin_event = threading.Event() + + # Start a new thread for spinning + self.spin_thread = threading.Thread(target=self.spin) + self.spin_thread.start() + + def spin(self): + while not self.spin_event.is_set(): + rclpy.spin_once(self, timeout_sec=0.1) + def update_curr_pose(self, msg): + # self.curr_pose + self.curr_pose = msg + + def goalReached_callback(self,msg): + if msg.data: + self.goalReached = True + + def wait_for_goal(self, timeout=60.0): + start_time = datetime.datetime.now() + while not self.goalReached: + elapsed_time = (datetime.datetime.now() - start_time).total_seconds() + if elapsed_time > timeout: + self.get_logger().warn('Timeout while waiting for goal to be reached') + return False + sleep(0.001) + return True + + def is_close_pos(self, xyz_array, pos_thresh = 0.0005): + translation_vec = np.asarray([xyz_array[0] - self.curr_pose.position.x, + xyz_array[1] - self.curr_pose.position.y, + xyz_array[2] - self.curr_pose.position.z]) + + return np.linalg.norm(translation_vec) 0.75 or command_pose.position.x < 0.35 or - command_pose.position.y > 0.1 or command_pose.position.y < -0.1 or - command_pose.position.z > 0.78 or command_pose.position.z < 0.5): #Command guarding - - print('Failed, not executable') - - else: - self.pose_pub.publish(command_pose) - - - def generate_move_command(self, lin_vel, pos_thresh = 0.0005): #Generates move commands, for motions containing ONLY rotational movements use generate_move_command_rotation method + self.desired_pose = msg + print('updating!') + + def move_to_pose_callback(self, request, response): + self.goal_pose = request.goal_pose + self.moving_event.set() + response.success = True + return response + + def move_robot(self): + while rclpy.ok(): + self.moving_event.wait() # Wait until the event is set + while not (self.is_close_pos() and self.is_close_orien()): + if not self.is_close_pos(): + lin_vel = 0.005 + command_pose = self.generate_move_command(lin_vel) + else: + MotionTime = 20.0 + command_pose = self.generate_move_command_rotation(MotionTime) + + if self.is_safe_pose(command_pose): + self.pose_pub.publish(command_pose) + print(command_pose.position) + else: + self.moving_event.clear() + break + + time.sleep(self.communication_rate) + + self.moving_event.clear() + + def generate_move_command(self, lin_vel, pos_thresh=0.0005): #Generates move commands, for motions containing ONLY rotational movements use generate_move_command_rotation method command_pose = Pose() GoalPose = self.goal_pose CurrPose = self.curr_pose @@ -110,6 +90,7 @@ def generate_move_command(self, lin_vel, pos_thresh = 0.0005): #Generates move c goal_Rot = Rotation(GoalPose.orientation) ABC_diff = goal_Rot.as_ABC() - (Rotation(self.desired_pose.orientation)).as_ABC() for i in range(3): + if(ABC_diff[i]>np.pi): ABC_diff[i]-=2.0*np.pi elif(ABC_diff[i]<(-np.pi)): @@ -155,14 +136,6 @@ def generate_move_command_rotation(self, motion_time, angle_thresh = 0.1*3.1415/ command_pose.orientation = quat_command.as_geometry_orientation() - - # if(np.max(np.abs(ABC_command - curr_Rot.as_ABC()))>0.2*3.14159/180.0): - # if(int(round(time.time() * 1000))%5==0): - # print(ABC_command) - # print(curr_Rot.as_ABC()) - # print((Rotation(self.desired_pose.orientation)).as_ABC()) - # print('Too much jump') - # return False self.last_command = command_pose self.desired_pose = copy.deepcopy(command_pose) @@ -196,6 +169,13 @@ def is_close_orien(self, angle_thresh = 0.1*3.1415/180.0): return np.max(np.abs(ABC_diff)) 0.6 or pose.position.x < 0.4 or + pose.position.y > 0.1 or pose.position.y < -0.1 or + pose.position.z > 0.6 or pose.position.z < 0.4): + print('Failed, not executable') + return False + return True def main(args=None): rclpy.init(args=args) @@ -205,4 +185,4 @@ def main(args=None): rclpy.shutdown() if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/move_to_pose_client.py b/lbr_demos/lbr_demos_py/lbr_demos_py/move_to_pose_client.py new file mode 100644 index 00000000..d84bf361 --- /dev/null +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/move_to_pose_client.py @@ -0,0 +1,47 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Pose +from lbr_fri_idl.srv import MoveToPose +from lbr_demos_py.asbr import * + + +class MoveToPoseClient(Node): + + def __init__(self): + super().__init__('move_to_pose_client') + self.client = self.create_client(MoveToPose, 'move_to_pose') + + while not self.client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Service not available, waiting again...') + + self.request = MoveToPose.Request() + + def send_request(self, goal_pose): + self.request.goal_pose = goal_pose + self.future = self.client.call_async(self.request) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result() + +def main(args=None): + rclpy.init(args=args) + client = MoveToPoseClient() + + goal_pose = Pose() + goal_pose.position.x = 0.5 + goal_pose.position.y = 0.0 + goal_pose.position.z = 0.5 + # Set orientation as required + + goal_orientation = [180, 10, 180] + goal_orientation = Rotation.from_ABC(goal_orientation,True) + goal_orientation = goal_orientation.as_geometry_orientation() + goal_pose.orientation = goal_orientation + + response = client.send_request(goal_pose) + print(f'Success: {response.success}') + + client.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/lbr_demos/lbr_demos_py/package.xml b/lbr_demos/lbr_demos_py/package.xml index 67ddd0c4..e6cd00a3 100644 --- a/lbr_demos/lbr_demos_py/package.xml +++ b/lbr_demos/lbr_demos_py/package.xml @@ -14,6 +14,9 @@ lbr_ros2_control rclpy trajectory_msgs + + geometry_msgs + rosidl_default_runtime ament_copyright ament_flake8 @@ -23,4 +26,4 @@ ament_python - \ No newline at end of file + diff --git a/lbr_demos/lbr_demos_py/setup.cfg b/lbr_demos/lbr_demos_py/setup.cfg index 2056f2c7..8dbec21d 100644 --- a/lbr_demos/lbr_demos_py/setup.cfg +++ b/lbr_demos/lbr_demos_py/setup.cfg @@ -2,3 +2,22 @@ script_dir=$base/lib/lbr_demos_py [install] install_scripts=$base/lib/lbr_demos_py + +[options] +packages = lbr_demos_py +install_requires = setuptools +data_files = + share/ament_index/resource_index/packages, + resource/lbr_demos_py + share/lbr_demos_py/package.xml, + share/lbr_demos_py/srv/MoveToPose.srv + +[options.entry_points] +console_scripts = + joint_sine_overlay = lbr_demos_py.joint_sine_overlay:main + joint_trajectory_client = lbr_demos_py.joint_trajectory_client:main + torque_sine_overlay = lbr_demos_py.torque_sine_overlay:main + wrench_sine_overlay = lbr_demos_py.wrench_sine_overlay:main + pose_planning = lbr_demos_py.pose_planning:main + move_2_cart = lbr_demos_py.move_2_cart:main + move_to_pose_client = lbr_demos_py.move_to_pose_client:main diff --git a/lbr_demos/lbr_demos_py/setup.py b/lbr_demos/lbr_demos_py/setup.py index 8837756c..93510d34 100644 --- a/lbr_demos/lbr_demos_py/setup.py +++ b/lbr_demos/lbr_demos_py/setup.py @@ -25,6 +25,7 @@ "wrench_sine_overlay = lbr_demos_py.wrench_sine_overlay:main", "pose_planning = lbr_demos_py.pose_planning:main", "move_2_cart = lbr_demos_py.move_2_cart:main", + "move_to_pose_client = lbr_demos_py.move_to_pose_client:main", ], }, )