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