def test_forward_position_kinematics(self):
arm = ArmInterface()
jnt_array = arm.home
pose = arm.forward_position_kinematics(arm.home)
self.assertLess(np.abs(pose[0:3] - HOME_CART_POS).sum(), 1e-6, 'Invalid output position, pos=[%.8f, %.8f, %.8f]' % (pose[0], pose[1], pose[2]))
self.assertLess(np.abs(pose[3::] - HOME_CART_ROT).sum(), 1e-6, 'Invalid output orientation, rot=[%.8f, %.8f, %.8f, %.8f]' % (pose[3], pose[4], pose[5], pose[6]))
def test_joints_to_kdl(self):
arm = ArmInterface()
for idx, name in zip(range(len(arm.joint_names)), arm.joint_names):
for t in ['positions', 'torques']:
jnt_array = arm.joints_to_kdl(t, last_joint=name)
self.assertEquals(jnt_array.rows(), idx + 1,
'Invalid number of joints, joint_idx=%d, last_joint=%s, n_joints=%d' % (idx, name, jnt_array.rows()))
def test_jacobian(self):
arm = ArmInterface()
jac = arm.jacobian()
self.assertIsNotNone(jac, 'Jacobian matrix is invalid')
self.assertEquals(jac.shape, (arm.n_links - 1, 6), 'The full Jacobian matrix has the wrong size')
for idx, name in zip(range(len(arm.joint_names)), arm.joint_names):
self.assertEquals(arm.jacobian(last_joint=name).shape, (arm.n_links - 1, idx + 1))
self.assertEquals(arm.jacobian_transpose(last_joint=name).shape, (idx + 1, arm.n_links - 1))
def test_inv_kinematics(self):
arm = ArmInterface()
home_q = arm.home
q = arm.inverse_kinematics(HOME_CART_POS, HOME_CART_ROT)
for idx in range(len(arm.joint_names)):
self.assertLess(
np.abs(home_q[arm.joint_names[idx]] - q[idx]), 1e-6,
'home_q=%.4f, q=%.4f' % (home_q[arm.joint_names[idx]], q[idx]))
def test_joints_to_kdl(self):
arm = ArmInterface()
for idx, name in zip(range(len(arm.joint_names)), arm.joint_names):
for t in ['positions', 'torques']:
jnt_array = arm.joints_to_kdl(t, last_joint=name)
self.assertEquals(
jnt_array.rows(), idx + 1,
'Invalid number of joints, joint_idx=%d, last_joint=%s, n_joints=%d'
% (idx, name, jnt_array.rows()))
def __init__(self):
ns = [
item for item in rospy.get_namespace().split('/') if len(item) > 0
]
if len(ns) != 2:
rospy.ROSException(
'The controller must be called in the manipulator namespace')
self._namespace = ns[0]
self._arm_name = ns[1]
if self._namespace[0] != '/':
self._namespace = '/' + self._namespace
if self._namespace[-1] != '/':
self._namespace += '/'
# The arm interface loads the parameters from the URDF file and
# parameter server and initializes the KDL tree
self._arm_interface = ArmInterface()
self._base_link = self._arm_interface.base_link
self._tip_link = self._arm_interface.tip_link
self._tip_frame = PyKDL.Frame()
# KDL Solvers
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_interface.chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(
self._arm_interface.chain, self._arm_interface._fk_p_kdl,
self._ik_v_kdl, 100, 1e-6)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_interface.chain,
PyKDL.Vector.Zero())
# Add a callback function to calculate the end effector's state by each
# update of the joint state
self._arm_interface.add_callback('joint_states',
self.on_update_endeffector_state)
# Publish the current manipulability index at each update of the joint
# states
# self._arm_interface.add_callback('joint_states',
# self.publish_man_index)
# Publish the end effector state
self._endeffector_state_pub = rospy.Publisher('endpoint_state',
EndPointState,
queue_size=1)
# Publish the manipulability index
self._man_index_pub = rospy.Publisher('man_index',
Float64,
queue_size=1)
self._services = dict()
# Provide the service to calculate the inverse kinematics using the KDL solver
self._services['ik'] = rospy.Service('ik_solver', SolveIK,
self.solve_ik)
def test_jacobian(self):
arm = ArmInterface()
jac = arm.jacobian()
self.assertIsNotNone(jac, 'Jacobian matrix is invalid')
self.assertEquals(jac.shape, (arm.n_links, 6),
'The full Jacobian matrix has the wrong size')
for idx, name in zip(range(len(arm.link_names)), arm.link_names):
self.assertEquals(
arm.jacobian(end_link=name).shape, (arm.n_links, 6))
self.assertEquals(
arm.jacobian_transpose(end_link=name).shape, (6, arm.n_links))
def test_forward_position_kinematics(self):
arm = ArmInterface()
jnt_array = arm.home
pose = arm.forward_position_kinematics(arm.home)
self.assertLess(
np.abs(pose[0:3] - HOME_CART_POS).sum(), 1e-6,
'Invalid output position, pos=[%.8f, %.8f, %.8f]' %
(pose[0], pose[1], pose[2]))
self.assertLess(
np.abs(pose[3::] - HOME_CART_ROT).sum(), 1e-6,
'Invalid output orientation, rot=[%.8f, %.8f, %.8f, %.8f]' %
(pose[3], pose[4], pose[5], pose[6]))
def test_init_interface(self):
arm = ArmInterface()
# Test if the namespace and arm name are correct
self.assertEquals(arm.namespace, '/rexrov/', 'Invalid robot namespace')
self.assertEquals(arm.arm_name, 'oberon', 'Invalid arm name')
self.assertEquals(arm.base_link, 'oberon/base',
'Invalid manipulator base name')
self.assertEquals(arm.tip_link, 'oberon/end_effector',
'Invalid end-effector link name')
self.assertNotEquals(len(arm.joint_names), 0,
'The list of joint names is empty')
self.assertEquals(arm.n_links, 7, 'Invalid number of links')
for name in arm.joint_names:
self.assertIn(
name, arm.joint_angles,
'Joint name %s not listed in the joint positions dictionary' %
name)
self.assertIn(
name, arm.joint_velocities,
'Joint name %s not listed in the joint velocities dictionary' %
name)
self.assertIn(
name, arm.joint_efforts,
'Joint name %s not listed in the joint efforts dictionary' %
name)
def __init__(self):
# Timeout (to filter out inactivity)
self._timeout = 0.5
# Initializing the arm interface for the manipulator in the current
# namespace
self._arm_interface = ArmInterface()
# Retrieve the publish rate
self._publish_rate = 25
if not rospy.has_param('cartesian_controller/publish_rate'):
self._publish_rate = rospy.get_param(
'cartesian_controller/publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
# Check if orientation will be controlled
self._position_only = False
if rospy.has_param('~position_only'):
self._position_only = bool(rospy.get_param('~position_only'))
# Check if cylindrical coordinates are to be used
self._is_cylindrical = False
if rospy.has_param('~is_cylindrical'):
self._is_cylindrical = bool(rospy.get_param('~is_cylindrical'))
# Get the transformation between the robot's base link and the
# vehicle's base link
self.listener = tf.TransformListener()
# Get latest transform available
latest = rospy.Time(0)
base = self._arm_interface.namespace + 'base_link'
self.listener.waitForTransform(base, self._arm_interface.base_link,
latest, latest + rospy.Duration(100))
[pos,
quat] = self.listener.lookupTransform(base,
self._arm_interface.base_link,
latest)
rot = PyKDL.Rotation.Quaternion(quat[0], quat[1], quat[2], quat[3])
# Store transformation from the arm's base link and base
self._trans = PyKDL.Frame(rot, PyKDL.Vector(pos[0], pos[1], pos[2]))
# Velocity reference
self._command = None
# Subscribe to the twist reference topic
self._command_sub = rospy.Subscriber('cartesian_controller/command',
TwistStamped,
self._command_callback,
queue_size=1)
# Last controller update
self._last_controller_update = rospy.get_time()
# Last goal for the end-effector pose/position
self._last_goal = None
try:
# Wait for the IK solver service
rospy.wait_for_service('ik_solver', timeout=2)
except rospy.ROSException, e:
print 'Service not available'
class KinematicsService(object):
def __init__(self):
ns = [
item for item in rospy.get_namespace().split('/') if len(item) > 0
]
if len(ns) != 2:
rospy.ROSException(
'The controller must be called in the manipulator namespace')
self._namespace = ns[0]
self._arm_name = ns[1]
if self._namespace[0] != '/':
self._namespace = '/' + self._namespace
if self._namespace[-1] != '/':
self._namespace += '/'
# The arm interface loads the parameters from the URDF file and
# parameter server and initializes the KDL tree
self._arm_interface = ArmInterface()
self._base_link = self._arm_interface.base_link
self._tip_link = self._arm_interface.tip_link
self._tip_frame = PyKDL.Frame()
# KDL Solvers
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_interface.chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(
self._arm_interface.chain, self._arm_interface._fk_p_kdl,
self._ik_v_kdl, 100, 1e-6)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_interface.chain,
PyKDL.Vector.Zero())
# Add a callback function to calculate the end effector's state by each
# update of the joint state
self._arm_interface.add_callback('joint_states',
self.on_update_endeffector_state)
# Publish the current manipulability index at each update of the joint
# states
# self._arm_interface.add_callback('joint_states',
# self.publish_man_index)
# Publish the end effector state
self._endeffector_state_pub = rospy.Publisher('endpoint_state',
EndPointState,
queue_size=1)
# Publish the manipulability index
self._man_index_pub = rospy.Publisher('man_index',
Float64,
queue_size=1)
self._services = dict()
# Provide the service to calculate the inverse kinematics using the KDL solver
self._services['ik'] = rospy.Service('ik_solver', SolveIK,
self.solve_ik)
@property
def joint_names(self):
return self._arm_interface.joint_names
@property
def joint_angles(self):
return self._arm_interface.joint_angles
@property
def joint_velocities(self):
return self._arm_interface.joint_velocities
@property
def joint_efforts(self):
return self._arm_interface.joint_efforts
@property
def home(self):
return self._arm_interface.home
def solve_ik(self, req):
out = SolveIKResponse()
out.isValid = False
out.joints = JointState()
pos = [req.pose.position.x, req.pose.position.y, req.pose.position.z]
orientation = [
req.pose.orientation.x, req.pose.orientation.y,
req.pose.orientation.z, req.pose.orientation.w
]
result_ik = self._arm_interface.inverse_kinematics(pos, orientation)
if result_ik is not None:
for i, name in zip(range(len(self.joint_names)), self.joint_names):
out.joints.name.append(name)
out.joints.position.append(result_ik[i])
out.isValid = True
return out
def publish_man_index(self):
# Retrieve current jacobian matrix
w_msg = Float64()
w_msg.data = self._arm_interface.man_index
self._man_index_pub.publish(w_msg)
def on_update_endeffector_state(self):
state_msg = EndPointState()
# Store everything in the end point state message
state_msg.pose.position.x = self._arm_interface.endeffector_pose[
'position'][0]
state_msg.pose.position.y = self._arm_interface.endeffector_pose[
'position'][1]
state_msg.pose.position.z = self._arm_interface.endeffector_pose[
'position'][2]
state_msg.pose.orientation.x = self._arm_interface.endeffector_pose[
'orientation'][0]
state_msg.pose.orientation.y = self._arm_interface.endeffector_pose[
'orientation'][1]
state_msg.pose.orientation.z = self._arm_interface.endeffector_pose[
'orientation'][2]
state_msg.pose.orientation.w = self._arm_interface.endeffector_pose[
'orientation'][3]
state_msg.twist.linear.x = self._arm_interface.endeffector_twist[
'linear'][0]
state_msg.twist.linear.y = self._arm_interface.endeffector_twist[
'linear'][1]
state_msg.twist.linear.z = self._arm_interface.endeffector_twist[
'linear'][2]
state_msg.twist.angular.x = self._arm_interface.endeffector_twist[
'angular'][0]
state_msg.twist.angular.y = self._arm_interface.endeffector_twist[
'angular'][1]
state_msg.twist.angular.z = self._arm_interface.endeffector_twist[
'angular'][2]
state_msg.wrench.force.x = self._arm_interface.endeffector_wrench[
'force'][0]
state_msg.wrench.force.y = self._arm_interface.endeffector_wrench[
'force'][1]
state_msg.wrench.force.z = self._arm_interface.endeffector_wrench[
'force'][2]
state_msg.wrench.torque.x = self._arm_interface.endeffector_wrench[
'torque'][0]
state_msg.wrench.torque.y = self._arm_interface.endeffector_wrench[
'torque'][0]
state_msg.wrench.torque.z = self._arm_interface.endeffector_wrench[
'torque'][0]
self._endeffector_state_pub.publish(state_msg)
def __init__(self):
# Initializing arm interface
self._arm_interface = ArmInterface()
# PID controllers
self._controllers = dict()
# Current reference for each joint
self._reference_pos = dict()
# Output command topics
self._command_topics = dict()
# Axes mapping
self._axes = dict()
# Axes gain values
self._axes_gain = dict()
# Default for the RB button of the XBox 360 controller
self._deadman_button = 5
if rospy.has_param('~deadman_button'):
self._deadman_button = int(rospy.get_param('~deadman_button'))
# If these buttons are pressed, the arm will not move
if rospy.has_param('~exclusion_buttons'):
self._exclusion_buttons = rospy.get_param('~exclusion_buttons')
if type(self._exclusion_buttons) in [float, int]:
self._exclusion_buttons = [int(self._exclusion_buttons)]
elif type(self._exclusion_buttons) == list:
for n in self._exclusion_buttons:
if type(n) not in [float, int]:
raise rospy.ROSException(
'Exclusion buttons must be an'
' integer index to the joystick button')
else:
self._exclusion_buttons = list()
# Default for the start button of the XBox 360 controller
self._home_button = 7
if rospy.has_param('~home_button'):
self._home_button = int(rospy.get_param('~home_button'))
# Last joystick update timestamp
self._last_joy_update = rospy.get_time()
# Joystick topic subscriber
self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_callback)
# Reading the controller configuration
controller_config = rospy.get_param('~controller_config')
for joint in self._arm_interface.joint_names:
for tag in controller_config:
if tag in joint:
try:
# Read the controller parameters
self._controllers[joint] = PIDRegulator(
controller_config[tag]['controller']['p'],
controller_config[tag]['controller']['i'],
controller_config[tag]['controller']['d'], 1000)
self._command_topics[joint] = rospy.Publisher(
controller_config[tag]['topic'],
Float64,
queue_size=1)
self._axes[joint] = controller_config[tag][
'joint_input_axis']
self._axes_gain[joint] = controller_config[tag][
'axis_gain']
# Setting the starting reference to the home position
# in the robot parameters file
self._reference_pos[joint] = deepcopy(
self._arm_interface.home[joint])
except:
raise rospy.ROSException(
'Error while trying to setup controller for joint <%s>'
% joint)
rate = rospy.Rate(50)
while not rospy.is_shutdown():
pos = self._arm_interface.joint_angles
for joint in pos:
u = self._controllers[joint].regulate(
self._reference_pos[joint] - pos[joint], rospy.get_time())
self._command_topics[joint].publish(Float64(u))
rate.sleep()
def test_home_config(self):
arm = ArmInterface()
self.assertIsNotNone(arm.home, 'Home configuration is invalid')
def test_inv_kinematics(self):
arm = ArmInterface()
home_q = arm.home
q = arm.inverse_kinematics(HOME_CART_POS, HOME_CART_ROT)
for idx in range(len(arm.joint_names)):
self.assertLess(np.abs(home_q[arm.joint_names[idx]] - q[idx]), 1e-6, 'home_q=%.4f, q=%.4f' % (home_q[arm.joint_names[idx]], q[idx]))
def __init__(self):
# Timeout (to filter out inactivity)
self._timeout = 0.5
# Initializing the arm interface for the manipulator in the current
# namespace
self._arm_interface = ArmInterface()
# Last goal for the end-effector pose/position
self._last_goal = self._arm_interface.get_config_in_ee_frame('home')
# Retrieve the publish rate
self._publish_rate = 25
if rospy.has_param('~publish_rate'):
self._publish_rate = rospy.get_param('~publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
self._tau = 0.1
# Get the transformation between the robot's base link and the
# vehicle's base link
self.listener = tf.TransformListener()
# Get latest transform available
latest = rospy.Time(0)
base = self._arm_interface.namespace + 'base_link'
self.listener.waitForTransform(base, self._arm_interface.base_link,
latest, latest + rospy.Duration(100))
[pos, quat] = self.listener.lookupTransform(
base, self._arm_interface.base_link, latest)
rot = PyKDL.Rotation.Quaternion(quat[0], quat[1], quat[2], quat[3])
# Store transformation from the arm's base link and base
self._trans = PyKDL.Frame(rot, PyKDL.Vector(pos[0], pos[1], pos[2]))
# Velocity reference
self._command = None
# Last controller update
self._last_controller_update = rospy.get_time()
# Last velocity reference update time stamp
self._last_reference_update = rospy.get_time()
rospy.set_param('~name', self.LABEL)
self._joint_effort_pub = dict()
for joint in self._arm_interface.joint_names:
self._joint_effort_pub[joint] = rospy.Publisher(
self._arm_interface.namespace +
joint + '/controller/command',
Float64, queue_size=1)
# Input velocity command subscriber, remap this topic to set a custom
# command topic
self._vel_command_sub = rospy.Subscriber(
'cmd_vel', Twist, self._vel_command_callback)
# Topic that will receive the flag if the home button was pressed. If
# the flag is true, the manipulator's goal is set to the stow
# configuration
self._home_pressed_sub = rospy.Subscriber(
'home_pressed', Bool, self._home_button_pressed)
# Topic publishes the current goal set as reference to the manipulator
self._goal_pub = rospy.Publisher(
'reference', PoseStamped, queue_size=1)
# Topic to publish a visual marker for visualization of the current
# reference in RViz
self._goal_marker_pub = rospy.Publisher(
'reference_marker', Marker, queue_size=1)
class CartesianController(object):
LABEL = 'None'
def __init__(self):
# Timeout (to filter out inactivity)
self._timeout = 0.5
# Initializing the arm interface for the manipulator in the current
# namespace
self._arm_interface = ArmInterface()
# Last goal for the end-effector pose/position
self._last_goal = self._arm_interface.get_config_in_ee_frame('home')
# Retrieve the publish rate
self._publish_rate = 25
if rospy.has_param('~publish_rate'):
self._publish_rate = rospy.get_param('~publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
self._tau = 0.1
# Get the transformation between the robot's base link and the
# vehicle's base link
self.listener = tf.TransformListener()
# Get latest transform available
latest = rospy.Time(0)
base = self._arm_interface.namespace + 'base_link'
# self.listener.waitForTransform(base, self._arm_interface.base_link,
# latest, latest + rospy.Duration(100))
self.listener.waitForTransform(base, self._arm_interface.base_link,
latest, rospy.Duration(100))
[pos,
quat] = self.listener.lookupTransform(base,
self._arm_interface.base_link,
latest)
rot = PyKDL.Rotation.Quaternion(quat[0], quat[1], quat[2], quat[3])
# Store transformation from the arm's base link and base
self._trans = PyKDL.Frame(rot, PyKDL.Vector(pos[0], pos[1], pos[2]))
# Velocity reference
self._command = None
# Last controller update
self._last_controller_update = rospy.get_time()
# Last velocity reference update time stamp
self._last_reference_update = rospy.get_time()
rospy.set_param('~name', self.LABEL)
self._joint_effort_pub = dict()
for joint in self._arm_interface.joint_names:
self._joint_effort_pub[joint] = rospy.Publisher(
self._arm_interface.namespace + joint + '/controller/command',
Float64,
queue_size=1)
# Input velocity command subscriber, remap this topic to set a custom
# command topic
self._vel_command_sub = rospy.Subscriber('cmd_vel', Twist,
self._vel_command_callback)
# Topic that will receive the flag if the home button was pressed. If
# the flag is true, the manipulator's goal is set to the stow
# configuration
self._home_pressed_sub = rospy.Subscriber('home_pressed', Bool,
self._home_button_pressed)
# Topic publishes the current goal set as reference to the manipulator
self._goal_pub = rospy.Publisher('reference',
PoseStamped,
queue_size=1)
# Topic to publish a visual marker for visualization of the current
# reference in RViz
self._goal_marker_pub = rospy.Publisher('reference_marker',
Marker,
queue_size=1)
def _update(self, event):
raise NotImplementedError()
def _run(self):
rate = rospy.Rate(self._publish_rate)
while not rospy.is_shutdown():
self._update()
rate.sleep()
def _filter_input(self, state, cmd, dt):
alpha = np.exp(-1 * dt / self._tau)
return state * alpha + (1.0 - alpha) * cmd
def _get_goal(self):
if self._command is None or rospy.get_time(
) - self._last_reference_update > 0.1:
return self._last_goal
next_goal = deepcopy(self._last_goal)
next_goal.p += PyKDL.Vector(self._command[0], self._command[1],
self._command[2])
q_step = trans.quaternion_from_euler(self._command[3],
self._command[4],
self._command[5])
q_last = trans.quaternion_from_euler(*self._last_goal.M.GetRPY())
q_next = trans.quaternion_multiply(q_last, q_step)
next_goal.M = PyKDL.Rotation.Quaternion(*q_next)
g_pos = [next_goal.p.x(), next_goal.p.y(), next_goal.p.z()]
g_quat = next_goal.M.GetQuaternion()
if self._arm_interface.inverse_kinematics(g_pos, g_quat) is not None:
return next_goal
else:
print 'Next goal could not be resolved by the inv. kinematics solver.'
return self._last_goal
def _home_button_pressed(self, msg):
if msg.data:
self._command = np.zeros(6)
self._last_goal = self._arm_interface.get_config_in_ee_frame(
'home')
def _vel_command_callback(self, msg):
dt = rospy.get_time() - self._last_reference_update
if dt > 0.1:
self._command = np.zeros(6)
self._last_reference_update = rospy.get_time()
return
if self._command is None:
self._command = np.zeros(6)
self._command[0] = self._filter_input(self._command[0], msg.linear.x,
dt) * dt
self._command[1] = self._filter_input(self._command[1], msg.linear.y,
dt) * dt
self._command[2] = self._filter_input(self._command[2], msg.linear.z,
dt) * dt
self._command[3] = self._filter_input(self._command[3], msg.angular.x,
dt) * dt
self._command[4] = self._filter_input(self._command[4], msg.angular.y,
dt) * dt
self._command[5] = self._filter_input(self._command[5], msg.angular.z,
dt) * dt
self._last_reference_update = rospy.get_time()
def publish_goal(self):
# Publish the reference topic
msg = PoseStamped()
msg.header.frame_id = self._arm_interface.base_link
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = self._last_goal.p.x()
msg.pose.position.y = self._last_goal.p.y()
msg.pose.position.z = self._last_goal.p.z()
msg.pose.orientation = Quaternion(*self._last_goal.M.GetQuaternion())
self._goal_pub.publish(msg)
marker = Marker()
marker.header.frame_id = self._arm_interface.base_link
marker.header.stamp = rospy.Time.now()
marker.ns = self._arm_interface.arm_name
marker.id = 0
marker.type = Marker.SPHERE
marker.action = Marker.MODIFY
marker.pose.position = Vector3(self._last_goal.p.x(),
self._last_goal.p.y(),
self._last_goal.p.z())
marker.pose.orientation = Quaternion(
*self._last_goal.M.GetQuaternion())
marker.scale = Vector3(0.2, 0.2, 0.2)
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
self._goal_marker_pub.publish(marker)
def publish_joint_efforts(self, tau):
# Publish torques
t = np.asarray(tau).squeeze()
for i, name in enumerate(self._arm_interface.joint_names):
torque = Float64()
torque.data = t[i]
self._joint_effort_pub[name].publish(torque)
# Update the time stamp
self._last_controller_update = rospy.get_time()
def __init__(self):
# Timeout (to filter out inactivity)
self._timeout = 0.5
# Initializing the arm interface for the manipulator in the current
# namespace
self._arm_interface = ArmInterface()
# Last goal for the end-effector pose/position
self._last_goal = self._arm_interface.get_config_in_ee_frame('home')
# Retrieve the publish rate
self._publish_rate = 25
if not rospy.has_param('~cartesian_controller/publish_rate'):
self._publish_rate = rospy.get_param('~cartesian_controller/publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
self._t_step = 0.01
if rospy.has_param("~tstep"):
self._t_step = rospy.get_param('~tstep')
if self._t_step <= 0:
raise rospy.ROSException('Invalid translational step')
self._r_step = 0.01
if rospy.has_param("~rstep"):
self._r_step = rospy.get_param('~rstep')
if self._r_step <= 0:
raise rospy.ROSException('Invalid rotational step')
# Default mapping for XBox 360 controller
self._axes = dict(x=4, y=3, z=1, roll=0, pitch=7, yaw=6)
if rospy.has_param('~axes'):
axes = rospy.get_param('~axes')
if type(axes) != dict:
raise rospy.ROSException('Axes structure must be a dict')
for tag in self._axes:
if tag not in axes:
raise rospy.ROSException('Axes for %s missing' % tag)
self._axes[tag] = axes[tag]
# Default for the RB button of the XBox 360 controller
self._deadman_button = 5
if rospy.has_param('~deadman_button'):
self._deadman_button = int(rospy.get_param('~deadman_button'))
# If these buttons are pressed, the arm will not move
if rospy.has_param('~exclusion_buttons'):
self._exclusion_buttons = rospy.get_param('~exclusion_buttons')
if type(self._exclusion_buttons) in [float, int]:
self._exclusion_buttons = [int(self._exclusion_buttons)]
elif type(self._exclusion_buttons) == list:
for n in self._exclusion_buttons:
if type(n) not in [float, int]:
raise rospy.ROSException('Exclusion buttons must be an'
' integer index to the joystick button')
else:
self._exclusion_buttons = list()
# Default for the start button of the XBox 360 controller
self._home_button = 7
if rospy.has_param('~home_button'):
self._home_button = int(rospy.get_param('~home_button'))
# Get the transformation between the robot's base link and the
# vehicle's base link
self.listener = tf.TransformListener()
# Get latest transform available
latest = rospy.Time(0)
base = self._arm_interface.namespace + 'base_link'
self.listener.waitForTransform(base, self._arm_interface.base_link,
latest, latest + rospy.Duration(100))
[pos, quat] = self.listener.lookupTransform(
base, self._arm_interface.base_link, latest)
rot = PyKDL.Rotation.Quaternion(quat[0], quat[1], quat[2], quat[3])
# Store transformation from the arm's base link and base
self._trans = PyKDL.Frame(rot, PyKDL.Vector(pos[0], pos[1], pos[2]))
# Velocity reference
self._command = None
# Last controller update
self._last_controller_update = rospy.get_time()
rospy.set_param('~cartesian_controller/name', self.LABEL)
self._joint_effort_pub = dict()
for joint in self._arm_interface.joint_names:
self._joint_effort_pub[joint] = rospy.Publisher(
self._arm_interface.namespace +
joint + '/controller/command',
Float64, queue_size=1)
self._last_joy_update = rospy.get_time()
self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_callback)
def __init__(self):
# Timeout (to filter out inactivity)
self._timeout = 0.5
# Initializing the arm interface for the manipulator in the current
# namespace
self._arm_interface = ArmInterface()
# Last goal for the end-effector pose/position
self._last_goal = self._arm_interface.get_config_in_ee_frame('home')
# Retrieve the publish rate
self._publish_rate = 25
if rospy.has_param('~publish_rate'):
self._publish_rate = rospy.get_param('~publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
self._tau = 0.1
# Get the transformation between the robot's base link and the
# vehicle's base link
self.listener = tf.TransformListener()
# Get latest transform available
latest = rospy.Time(0)
base = self._arm_interface.namespace + 'base_link'
# self.listener.waitForTransform(base, self._arm_interface.base_link,
# latest, latest + rospy.Duration(100))
self.listener.waitForTransform(base, self._arm_interface.base_link,
latest, rospy.Duration(100))
[pos,
quat] = self.listener.lookupTransform(base,
self._arm_interface.base_link,
latest)
rot = PyKDL.Rotation.Quaternion(quat[0], quat[1], quat[2], quat[3])
# Store transformation from the arm's base link and base
self._trans = PyKDL.Frame(rot, PyKDL.Vector(pos[0], pos[1], pos[2]))
# Velocity reference
self._command = None
# Last controller update
self._last_controller_update = rospy.get_time()
# Last velocity reference update time stamp
self._last_reference_update = rospy.get_time()
rospy.set_param('~name', self.LABEL)
self._joint_effort_pub = dict()
for joint in self._arm_interface.joint_names:
self._joint_effort_pub[joint] = rospy.Publisher(
self._arm_interface.namespace + joint + '/controller/command',
Float64,
queue_size=1)
# Input velocity command subscriber, remap this topic to set a custom
# command topic
self._vel_command_sub = rospy.Subscriber('cmd_vel', Twist,
self._vel_command_callback)
# Topic that will receive the flag if the home button was pressed. If
# the flag is true, the manipulator's goal is set to the stow
# configuration
self._home_pressed_sub = rospy.Subscriber('home_pressed', Bool,
self._home_button_pressed)
# Topic publishes the current goal set as reference to the manipulator
self._goal_pub = rospy.Publisher('reference',
PoseStamped,
queue_size=1)
# Topic to publish a visual marker for visualization of the current
# reference in RViz
self._goal_marker_pub = rospy.Publisher('reference_marker',
Marker,
queue_size=1)
class CartesianController(object):
LABEL = 'None'
def __init__(self):
# Timeout (to filter out inactivity)
self._timeout = 0.5
# Initializing the arm interface for the manipulator in the current
# namespace
self._arm_interface = ArmInterface()
# Last goal for the end-effector pose/position
self._last_goal = self._arm_interface.get_config_in_ee_frame('home')
# Retrieve the publish rate
self._publish_rate = 25
if rospy.has_param('~publish_rate'):
self._publish_rate = rospy.get_param('~publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
self._tau = 0.1
# Get the transformation between the robot's base link and the
# vehicle's base link
self.listener = tf.TransformListener()
# Get latest transform available
latest = rospy.Time(0)
base = self._arm_interface.namespace + 'base_link'
self.listener.waitForTransform(base, self._arm_interface.base_link,
latest, latest + rospy.Duration(100))
[pos, quat] = self.listener.lookupTransform(
base, self._arm_interface.base_link, latest)
rot = PyKDL.Rotation.Quaternion(quat[0], quat[1], quat[2], quat[3])
# Store transformation from the arm's base link and base
self._trans = PyKDL.Frame(rot, PyKDL.Vector(pos[0], pos[1], pos[2]))
# Velocity reference
self._command = None
# Last controller update
self._last_controller_update = rospy.get_time()
# Last velocity reference update time stamp
self._last_reference_update = rospy.get_time()
rospy.set_param('~name', self.LABEL)
self._joint_effort_pub = dict()
for joint in self._arm_interface.joint_names:
self._joint_effort_pub[joint] = rospy.Publisher(
self._arm_interface.namespace +
joint + '/controller/command',
Float64, queue_size=1)
# Input velocity command subscriber, remap this topic to set a custom
# command topic
self._vel_command_sub = rospy.Subscriber(
'cmd_vel', Twist, self._vel_command_callback)
# Topic that will receive the flag if the home button was pressed. If
# the flag is true, the manipulator's goal is set to the stow
# configuration
self._home_pressed_sub = rospy.Subscriber(
'home_pressed', Bool, self._home_button_pressed)
# Topic publishes the current goal set as reference to the manipulator
self._goal_pub = rospy.Publisher(
'reference', PoseStamped, queue_size=1)
# Topic to publish a visual marker for visualization of the current
# reference in RViz
self._goal_marker_pub = rospy.Publisher(
'reference_marker', Marker, queue_size=1)
def _update(self, event):
raise NotImplementedError()
def _run(self):
rate = rospy.Rate(self._publish_rate)
while not rospy.is_shutdown():
self._update()
rate.sleep()
def _filter_input(self, state, cmd, dt):
alpha = np.exp(- 1 * dt / self._tau)
return state * alpha + (1.0 - alpha) * cmd
def _get_goal(self):
if self._command is None or rospy.get_time() - self._last_reference_update > 0.1:
return self._last_goal
next_goal = deepcopy(self._last_goal)
next_goal.p += PyKDL.Vector(self._command[0], self._command[1], self._command[2])
q_step = trans.quaternion_from_euler(self._command[3],
self._command[4],
self._command[5])
q_last = trans.quaternion_from_euler(*self._last_goal.M.GetRPY())
q_next = trans.quaternion_multiply(q_last, q_step)
next_goal.M = PyKDL.Rotation.Quaternion(*q_next)
g_pos = [next_goal.p.x(), next_goal.p.y(), next_goal.p.z()]
g_quat = next_goal.M.GetQuaternion()
if self._arm_interface.inverse_kinematics(g_pos, g_quat) is not None:
return next_goal
else:
print 'Next goal could not be resolved by the inv. kinematics solver.'
return self._last_goal
def _home_button_pressed(self, msg):
if msg.data:
self._command = np.zeros(6)
self._last_goal = self._arm_interface.get_config_in_ee_frame('home')
def _vel_command_callback(self, msg):
dt = rospy.get_time() - self._last_reference_update
if dt > 0.1:
self._command = np.zeros(6)
self._last_reference_update = rospy.get_time()
return
if self._command is None:
self._command = np.zeros(6)
self._command[0] = self._filter_input(self._command[0], msg.linear.x, dt) * dt
self._command[1] = self._filter_input(self._command[1], msg.linear.y, dt) * dt
self._command[2] = self._filter_input(self._command[2], msg.linear.z, dt) * dt
self._command[3] = self._filter_input(self._command[3], msg.angular.x, dt) * dt
self._command[4] = self._filter_input(self._command[4], msg.angular.y, dt) * dt
self._command[5] = self._filter_input(self._command[5], msg.angular.z, dt) * dt
self._last_reference_update = rospy.get_time()
def publish_goal(self):
# Publish the reference topic
msg = PoseStamped()
msg.header.frame_id = self._arm_interface.base_link
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = self._last_goal.p.x()
msg.pose.position.y = self._last_goal.p.y()
msg.pose.position.z = self._last_goal.p.z()
msg.pose.orientation = Quaternion(*self._last_goal.M.GetQuaternion())
self._goal_pub.publish(msg)
marker = Marker()
marker.header.frame_id = self._arm_interface.base_link
marker.header.stamp = rospy.Time.now()
marker.ns = self._arm_interface.arm_name
marker.id = 0
marker.type = Marker.SPHERE
marker.action = Marker.MODIFY
marker.pose.position = Vector3(self._last_goal.p.x(),
self._last_goal.p.y(),
self._last_goal.p.z())
marker.pose.orientation = Quaternion(*self._last_goal.M.GetQuaternion())
marker.scale = Vector3(0.2, 0.2, 0.2)
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
self._goal_marker_pub.publish(marker)
def publish_joint_efforts(self, tau):
# Publish torques
t = np.asarray(tau).squeeze()
for i, name in enumerate(self._arm_interface.joint_names):
torque = Float64()
torque.data = t[i]
self._joint_effort_pub[name].publish(torque)
# Update the time stamp
self._last_controller_update = rospy.get_time()
