def _get_initial_qpos(self):
"""
Calculates the initial joint position for the robot based on the initial desired pose (self.traj_pt, self.goal_quat).
Args:
Returns:
(np.array): n joint positions
"""
pos = self._convert_robosuite_to_toolbox_xpos(self.traj_pt)
ori_euler = mat2euler(quat2mat(self.goal_quat))
# desired pose
T = SE3(pos) * SE3.RPY(ori_euler)
# find initial joint positions
if self.robots[0].name == "UR5e":
robot = rtb.models.DH.UR5()
sol = robot.ikine_min(T, q0=self.robots[0].init_qpos)
# flip last joint around (pi)
sol.q[-1] -= np.pi
return sol.q
elif self.robots[0].name == "Panda":
robot = rtb.models.DH.Panda()
sol = robot.ikine_min(T, q0=self.robots[0].init_qpos)
return sol.q
def __init__(self,
robot: rtb.robot.Robot,
name: str = None,
joint_state_topic: str = None,
joint_velocity_topic: str = None,
origin=None,
config_path=None,
readonly=False,
gripper=None,
* args,
**kwargs): # pylint: disable=unused-argument
super().__init__(robot)
self.__dict__.update(robot.__dict__)
self.gripper=gripper
self.name = name if name else self.name
# Arm State information
# self.joint_names = list(map(lambda link: link.name.replace(
# 'link', 'joint'), filter(lambda link: link.isjoint, self.elinks)))
self.joint_names = list(map(lambda link: link._joint_name, filter(lambda link: link.isjoint, self.elinks)))
# self.joint_names =['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
self.joint_indexes = []
if origin:
self.base = SE3(origin[:3]) @ SE3.RPY(origin[3:])
self.q = self.qr if hasattr(self, 'qr') else self.q # pylint: disable=no-member
self.tau = [0] * robot.n
# Guards used to prevent multiple motion requests conflicting
self.moving: bool = False
self.last_moving: bool = False
self.preempted: bool = False
self.lock: Lock = Lock()
self.event: Event = Event()
# Arm state property
self.state: ManipulatorState = ManipulatorState()
self.e_v_frame: str = None # Expected cartesian velocity
self.e_v: np.array = np.zeros(shape=(6,)) # cartesian motion
self.j_v: np.array = np.zeros(
shape=(len(self.q),)
) # expected joint velocity
self.e_p = self.fkine(
self.q,
start=self.base_link,
end=self.gripper
) # measured end-effector position
self.last_update: float = 0
self.last_tick: float = 0
self.joint_subscriber = rospy.Subscriber(
joint_state_topic if joint_state_topic else '/joint_states',
JointState,
self._state_cb
)
self.readonly = readonly
self.joint_velocity_topic = joint_velocity_topic \
if joint_velocity_topic \
else '/joint_group_velocity_controller/command'
if not self.readonly:
self.joint_publisher = rospy.Publisher(
self.joint_velocity_topic,
Float64MultiArray,
queue_size=1
)
# Create Transform Listener
self.tf_listener = tf.TransformListener()
self.config_path = config_path if config_path else os.path.join(
os.getenv('HOME', '/root'),
'.ros/configs/armer.yaml'
)
self.custom_configs: List[str] = []
self.__load_config()
# Services
rospy.Service('{}/home'.format(self.name.lower()),
Empty, self.home_cb)
rospy.Service('{}/recover'.format(self.name.lower()),
Empty, self.recover_cb)
rospy.Service('{}/stop'.format(self.name.lower()),
Empty, self.preempt)
# Publishers
self.state_publisher: rospy.Publisher = rospy.Publisher(
'{}/state'.format(self.name.lower()), ManipulatorState, queue_size=1
)
# Subscribers
self.cartesian_velocity_subscriber: rospy.Subscriber = rospy.Subscriber(
'{}/cartesian/velocity'.format(self.name.lower()
), TwistStamped, self.velocity_cb
)
self.joint_velocity_subscriber: rospy.Subscriber = rospy.Subscriber(
'{}/joint/velocity'.format(self.name.lower()
), JointVelocity, self.joint_velocity_cb
)
# Action Servers
self.pose_server: actionlib.SimpleActionServer = actionlib.SimpleActionServer(
'{}/cartesian/pose'.format(self.name.lower()),
MoveToPoseAction,
execute_cb=self.pose_cb,
auto_start=False
)
self.pose_server.register_preempt_callback(self.preempt)
self.pose_server.start()
self.pose_servo_server: actionlib.SimpleActionServer = actionlib.SimpleActionServer(
'{}/cartesian/servo_pose'.format(self.name.lower()),
ServoToPoseAction,
execute_cb=self.servo_cb,
auto_start=False
)
self.pose_servo_server.register_preempt_callback(self.preempt)
self.pose_servo_server.start()
self.joint_pose_server: actionlib.SimpleActionServer = actionlib.SimpleActionServer(
'{}/joint/pose'.format(self.name.lower()),
MoveToJointPoseAction,
execute_cb=self.joint_pose_cb,
auto_start=False
)
self.joint_pose_server.register_preempt_callback(self.preempt)
self.joint_pose_server.start()
self.named_pose_server: actionlib.SimpleActionServer = actionlib.SimpleActionServer(
'{}/joint/named'.format(self.name.lower()),
MoveToNamedPoseAction,
execute_cb=self.named_pose_cb,
auto_start=False
)
self.named_pose_server.register_preempt_callback(self.preempt)
self.named_pose_server.start()
rospy.Service(
'{}/set_cartesian_impedance'.format(self.name.lower()),
SetCartesianImpedance,
self.set_cartesian_impedance_cb
)
rospy.Service('{}/get_named_poses'.format(self.name.lower()), GetNamedPoses,
self.get_named_poses_cb)
rospy.Service('{}/set_named_pose'.format(self.name.lower()), AddNamedPose,
self.add_named_pose_cb)
rospy.Service('{}/remove_named_pose'.format(self.name.lower()), RemoveNamedPose,
self.remove_named_pose_cb)
rospy.Service(
'{}/add_named_pose_config'.format(self.name.lower()),
AddNamedPoseConfig,
self.add_named_pose_config_cb
)
rospy.Service(
'{}/remove_named_pose_config'.format(self.name.lower()),
RemoveNamedPoseConfig,
self.remove_named_pose_config_cb
)
rospy.Service(
'{}/get_named_pose_configs'.format(self.name.lower()),
GetNamedPoseConfigs,
self.get_named_pose_configs_cb
)
def test_constructor(self):
# null constructor
R = SE3()
nt.assert_equal(len(R), 1)
array_compare(R, np.eye(4))
self.assertIsInstance(R, SE3)
# construct from matrix
R = SE3(trotx(0.2))
nt.assert_equal(len(R), 1)
array_compare(R, trotx(0.2))
self.assertIsInstance(R, SE3)
# construct from canonic rotation
R = SE3.Rx(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, trotx(0.2))
self.assertIsInstance(R, SE3)
R = SE3.Ry(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, troty(0.2))
self.assertIsInstance(R, SE3)
R = SE3.Rz(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, trotz(0.2))
self.assertIsInstance(R, SE3)
# construct from canonic translation
R = SE3.Tx(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, transl(0.2, 0, 0))
self.assertIsInstance(R, SE3)
R = SE3.Ty(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, transl(0, 0.2, 0))
self.assertIsInstance(R, SE3)
R = SE3.Tz(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, transl(0, 0, 0.2))
self.assertIsInstance(R, SE3)
# triple angle
R = SE3.Eul([0.1, 0.2, 0.3])
nt.assert_equal(len(R), 1)
array_compare(R, eul2tr([0.1, 0.2, 0.3]))
self.assertIsInstance(R, SE3)
R = SE3.Eul(np.r_[0.1, 0.2, 0.3])
nt.assert_equal(len(R), 1)
array_compare(R, eul2tr([0.1, 0.2, 0.3]))
self.assertIsInstance(R, SE3)
R = SE3.Eul([10, 20, 30], unit='deg')
nt.assert_equal(len(R), 1)
array_compare(R, eul2tr([10, 20, 30], unit='deg'))
self.assertIsInstance(R, SE3)
R = SE3.RPY([0.1, 0.2, 0.3])
nt.assert_equal(len(R), 1)
array_compare(R, rpy2tr([0.1, 0.2, 0.3]))
self.assertIsInstance(R, SE3)
R = SE3.RPY(np.r_[0.1, 0.2, 0.3])
nt.assert_equal(len(R), 1)
array_compare(R, rpy2tr([0.1, 0.2, 0.3]))
self.assertIsInstance(R, SE3)
R = SE3.RPY([10, 20, 30], unit='deg')
nt.assert_equal(len(R), 1)
array_compare(R, rpy2tr([10, 20, 30], unit='deg'))
self.assertIsInstance(R, SE3)
R = SE3.RPY([0.1, 0.2, 0.3], order='xyz')
nt.assert_equal(len(R), 1)
array_compare(R, rpy2tr([0.1, 0.2, 0.3], order='xyz'))
self.assertIsInstance(R, SE3)
# angvec
R = SE3.AngVec(0.2, [1, 0, 0])
nt.assert_equal(len(R), 1)
array_compare(R, trotx(0.2))
self.assertIsInstance(R, SE3)
R = SE3.AngVec(0.3, [0, 1, 0])
nt.assert_equal(len(R), 1)
array_compare(R, troty(0.3))
self.assertIsInstance(R, SE3)
# OA
R = SE3.OA([0, 1, 0], [0, 0, 1])
nt.assert_equal(len(R), 1)
array_compare(R, np.eye(4))
self.assertIsInstance(R, SE3)
# random
R = SE3.Rand()
nt.assert_equal(len(R), 1)
self.assertIsInstance(R, SE3)
# random
T = SE3.Rand()
R = T.R
t = T.t
T = SE3.Rt(R, t)
self.assertIsInstance(T, SE3)
self.assertEqual(T.A.shape, (4, 4))
nt.assert_equal(T.R, R)
nt.assert_equal(T.t, t)
# copy constructor
R = SE3.Rx(pi / 2)
R2 = SE3(R)
R = SE3.Ry(pi / 2)
array_compare(R2, trotx(pi / 2))
# SO3
T = SE3(SO3())
nt.assert_equal(len(T), 1)
self.assertIsInstance(T, SE3)
nt.assert_equal(T.A, np.eye(4))
# SE2
T = SE3(SE2(1, 2, 0.4))
nt.assert_equal(len(T), 1)
self.assertIsInstance(T, SE3)
self.assertEqual(T.A.shape, (4, 4))
nt.assert_equal(T.t, [1, 2, 0])
