00001
00002
00003 import numpy as np, math
00004 from threading import RLock, Timer
00005 import sys, copy
00006
00007
00008 import roslib; roslib.load_manifest('hrl_pr2_lib')
00009 roslib.load_manifest('force_torque')
00010 import force_torque.FTClient as ftc
00011
00012 import tf
00013 import hrl_lib.transforms as tr
00014 import hrl_lib.viz as hv
00015
00016 import rospy
00017 import PyKDL as kdl
00018
00019 import actionlib
00020 from actionlib_msgs.msg import GoalStatus
00021
00022 from kinematics_msgs.srv import GetPositionIK, GetPositionIKRequest, GetPositionIKResponse
00023 from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal, JointTrajectoryControllerState
00024 from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction, Pr2GripperCommand
00025
00026 from trajectory_msgs.msg import JointTrajectoryPoint
00027 from geometry_msgs.msg import PoseStamped
00028
00029 from teleop_controllers.msg import JTTeleopControllerState
00030
00031 from std_msgs.msg import Float64
00032 from sensor_msgs.msg import JointState
00033
00034 import hrl_lib.transforms as tr
00035 import hrl_lib.kdl_utils as ku
00036 import time
00037
00038 from visualization_msgs.msg import Marker
00039
00040
00041 node_name = "pr2_arms"
00042
00043 def log(str):
00044 rospy.loginfo(node_name + ": " + str)
00045
00046
00047
00048
00049
00050
00051 def make_column(x):
00052 if (type(x) == type([])
00053 or (type(x) == np.ndarray and x.ndim == 1)
00054 or type(x) == type(())):
00055 return np.matrix(x).T
00056 if type(x) == np.ndarray:
00057 x = np.matrix(x)
00058 if x.shape[0] == 1:
00059 return x.T
00060 return x
00061
00062
00063 class PR2Arms(object):
00064 def __init__(self, primary_ft_sensor):
00065 log("Loading PR2Arms")
00066
00067 self.arms = PR2Arms_kdl()
00068
00069 self.joint_names_list = [['r_shoulder_pan_joint',
00070 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint',
00071 'r_elbow_flex_joint', 'r_forearm_roll_joint',
00072 'r_wrist_flex_joint', 'r_wrist_roll_joint'],
00073 ['l_shoulder_pan_joint',
00074 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint',
00075 'l_elbow_flex_joint', 'l_forearm_roll_joint',
00076 'l_wrist_flex_joint', 'l_wrist_roll_joint']]
00077
00078 self.arm_state_lock = [RLock(), RLock()]
00079 self.jep = [None, None]
00080
00081 self.arm_angles = [None, None]
00082 self.torso_position = None
00083 self.arm_efforts = [None, None]
00084
00085 self.r_arm_cart_pub = rospy.Publisher('/r_cart/command_pose', PoseStamped)
00086 self.l_arm_cart_pub = rospy.Publisher('/l_cart/command_pose', PoseStamped)
00087
00088 rospy.Subscriber('/r_cart/state', JTTeleopControllerState, self.r_cart_state_cb)
00089 rospy.Subscriber('/l_cart/state', JTTeleopControllerState, self.l_cart_state_cb)
00090
00091 rospy.Subscriber('/joint_states', JointState,
00092 self.joint_states_cb, queue_size=2)
00093 self.marker_pub = rospy.Publisher('/pr2_arms/viz_markers', Marker)
00094 self.cep_marker_id = 1
00095
00096 self.r_arm_ftc = ftc.FTClient('force_torque_ft2')
00097 self.r_arm_ftc_estimate = ftc.FTClient('force_torque_ft2_estimate')
00098 self.tf_lstnr = tf.TransformListener()
00099
00100 if primary_ft_sensor == 'ati':
00101 self.get_wrist_force = self.get_wrist_force_ati
00102 if primary_ft_sensor == 'estimate':
00103 self.get_wrist_force = self.get_wrist_force_estimate
00104
00105 r_action_client = actionlib.SimpleActionClient('r_arm_controller/joint_trajectory_action',
00106 JointTrajectoryAction)
00107 l_action_client = actionlib.SimpleActionClient('l_arm_controller/joint_trajectory_action',
00108 JointTrajectoryAction)
00109 self.joint_action_client = [r_action_client, l_action_client]
00110
00111 r_gripper_client = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',
00112 Pr2GripperCommandAction)
00113 l_gripper_client = actionlib.SimpleActionClient('l_gripper_controller/gripper_action',
00114 Pr2GripperCommandAction)
00115 self.gripper_action_client = [r_gripper_client, l_gripper_client]
00116 rospy.sleep(2.)
00117
00118
00119
00120 self.gripper_action_client[0].wait_for_server()
00121 self.gripper_action_client[1].wait_for_server()
00122
00123 log("Finished loading SimpleArmManger")
00124
00125
00126
00127
00128
00129 def joint_states_cb(self, data):
00130 arm_angles = [[], []]
00131 arm_efforts = [[], []]
00132 r_jt_idx_list = [0]*7
00133 l_jt_idx_list = [0]*7
00134 for i, jt_nm in enumerate(self.joint_names_list[0]):
00135 r_jt_idx_list[i] = data.name.index(jt_nm)
00136 for i, jt_nm in enumerate(self.joint_names_list[1]):
00137 l_jt_idx_list[i] = data.name.index(jt_nm)
00138
00139
00140 for i in range(7):
00141 idx = r_jt_idx_list[i]
00142 if data.name[idx] != self.joint_names_list[0][i]:
00143 raise RuntimeError('joint angle name does not match. Expected: %s, Actual: %s i: %d'%('r_'+nm+'_joint', data.name[idx], i))
00144 arm_angles[0].append(data.position[idx])
00145 arm_efforts[0].append(data.effort[idx])
00146
00147 idx = l_jt_idx_list[i]
00148 if data.name[idx] != self.joint_names_list[1][i]:
00149 raise RuntimeError('joint angle name does not match. Expected: %s, Actual: %s i: %d'%('r_'+nm+'_joint', data.name[idx], i))
00150
00151 ang = data.position[idx]
00152 arm_angles[1] += [ang]
00153 arm_efforts[1] += [data.effort[idx]]
00154
00155 self.arm_state_lock[0].acquire()
00156 self.arm_angles[0] = arm_angles[0]
00157 self.arm_efforts[0] = arm_efforts[0]
00158
00159 torso_idx = data.name.index('torso_lift_joint')
00160 self.torso_position = data.position[torso_idx]
00161
00162 self.arm_state_lock[0].release()
00163
00164 self.arm_state_lock[1].acquire()
00165 self.arm_angles[1] = arm_angles[1]
00166 self.arm_efforts[1] = arm_efforts[1]
00167 self.arm_state_lock[1].release()
00168
00169 def r_cart_state_cb(self, msg):
00170 trans, quat = self.tf_lstnr.lookupTransform('/torso_lift_link',
00171 'r_gripper_tool_frame', rospy.Time(0))
00172 rot = tr.quaternion_to_matrix(quat)
00173 tip = np.matrix([0.12, 0., 0.]).T
00174 self.r_ee_pos = rot*tip + np.matrix(trans).T
00175 self.r_ee_rot = rot
00176
00177
00178 marker = Marker()
00179 marker.header.frame_id = 'torso_lift_link'
00180 time_stamp = rospy.Time.now()
00181 marker.header.stamp = time_stamp
00182 marker.ns = 'aloha land'
00183 marker.type = Marker.SPHERE
00184 marker.action = Marker.ADD
00185 marker.pose.position.x = self.r_ee_pos[0,0]
00186 marker.pose.position.y = self.r_ee_pos[1,0]
00187 marker.pose.position.z = self.r_ee_pos[2,0]
00188 size = 0.02
00189 marker.scale.x = size
00190 marker.scale.y = size
00191 marker.scale.z = size
00192 marker.lifetime = rospy.Duration()
00193
00194 marker.id = 71
00195 marker.pose.orientation.x = 0
00196 marker.pose.orientation.y = 0
00197 marker.pose.orientation.z = 0
00198 marker.pose.orientation.w = 1
00199
00200 color = (0.5, 0., 0.0)
00201 marker.color.r = color[0]
00202 marker.color.g = color[1]
00203 marker.color.b = color[2]
00204 marker.color.a = 1.
00205 self.marker_pub.publish(marker)
00206
00207 ros_pt = msg.x_desi_filtered.pose.position
00208 x, y, z = ros_pt.x, ros_pt.y, ros_pt.z
00209 self.r_cep_pos = np.matrix([x, y, z]).T
00210 pt = rot.T * (np.matrix([x,y,z]).T - np.matrix(trans).T)
00211 pt = pt + tip
00212 self.r_cep_pos_hooktip = rot*pt + np.matrix(trans).T
00213 ros_quat = msg.x_desi_filtered.pose.orientation
00214 quat = (ros_quat.x, ros_quat.y, ros_quat.z, ros_quat.w)
00215 self.r_cep_rot = tr.quaternion_to_matrix(quat)
00216
00217 def l_cart_state_cb(self, msg):
00218 ros_pt = msg.x_desi_filtered.pose.position
00219 self.l_cep_pos = np.matrix([ros_pt.x, ros_pt.y, ros_pt.z]).T
00220 ros_quat = msg.x_desi_filtered.pose.orientation
00221 quat = (ros_quat.x, ros_quat.y, ros_quat.z, ros_quat.w)
00222 self.l_cep_rot = tr.quaternion_to_matrix(quat)
00223
00224
00225
00226
00227
00228 def end_effector_pos(self, arm):
00229 q = self.get_joint_angles(arm)
00230 return self.arms.FK_all(arm, q)
00231
00232
00233
00234
00235 def get_joint_angles(self, arm):
00236 if arm != 1:
00237 arm = 0
00238 self.arm_state_lock[arm].acquire()
00239 q = self.arm_angles[arm]
00240 self.arm_state_lock[arm].release()
00241 return q
00242
00243 def set_jep(self, arm, q, duration=0.15):
00244 self.arm_state_lock[arm].acquire()
00245
00246 jtg = JointTrajectoryGoal()
00247 jtg.trajectory.joint_names = self.joint_names_list[arm]
00248 jtp = JointTrajectoryPoint()
00249 jtp.positions = q
00250
00251
00252 jtp.time_from_start = rospy.Duration(duration)
00253 jtg.trajectory.points.append(jtp)
00254 self.joint_action_client[arm].send_goal(jtg)
00255
00256 self.jep[arm] = q
00257 cep, r = self.arms.FK_all(arm, q)
00258 self.arm_state_lock[arm].release()
00259
00260 o = np.matrix([0.,0.,0.,1.]).T
00261 cep_marker = hv.single_marker(cep, o, 'sphere',
00262 '/torso_lift_link', color=(0., 0., 1., 1.),
00263 scale = (0.02, 0.02, 0.02),
00264 m_id = self.cep_marker_id)
00265
00266 cep_marker.header.stamp = rospy.Time.now()
00267 self.marker_pub.publish(cep_marker)
00268
00269 def get_jep(self, arm):
00270 self.arm_state_lock[arm].acquire()
00271 jep = copy.copy(self.jep[arm])
00272 self.arm_state_lock[arm].release()
00273 return jep
00274
00275 def get_ee_jtt(self, arm):
00276 if arm == 0:
00277 return self.r_ee_pos, self.r_ee_rot
00278 else:
00279 return self.l_ee_pos, self.l_ee_rot
00280
00281 def get_cep_jtt(self, arm, hook_tip = False):
00282 if arm == 0:
00283 if hook_tip:
00284 return self.r_cep_pos_hooktip, self.r_cep_rot
00285 else:
00286 return self.r_cep_pos, self.r_cep_rot
00287 else:
00288 return self.l_cep_pos, self.l_cep_rot
00289
00290
00291 def set_cep_jtt(self, arm, p, rot=None):
00292 if arm != 1:
00293 arm = 0
00294 ps = PoseStamped()
00295 ps.header.stamp = rospy.rostime.get_rostime()
00296 ps.header.frame_id = 'torso_lift_link'
00297
00298 ps.pose.position.x = p[0,0]
00299 ps.pose.position.y = p[1,0]
00300 ps.pose.position.z = p[2,0]
00301
00302 if rot == None:
00303 if arm == 0:
00304 rot = self.r_cep_rot
00305 else:
00306 rot = self.l_cep_rot
00307
00308 quat = tr.matrix_to_quaternion(rot)
00309 ps.pose.orientation.x = quat[0]
00310 ps.pose.orientation.y = quat[1]
00311 ps.pose.orientation.z = quat[2]
00312 ps.pose.orientation.w = quat[3]
00313 if arm == 0:
00314 self.r_arm_cart_pub.publish(ps)
00315 else:
00316 self.l_arm_cart_pub.publish(ps)
00317
00318
00319 def go_cep_jtt(self, arm, p):
00320 step_size = 0.01
00321 sleep_time = 0.1
00322 cep_p, cep_rot = self.get_cep_jtt(arm)
00323 unit_vec = (p-cep_p)
00324 unit_vec = unit_vec / np.linalg.norm(unit_vec)
00325 while np.linalg.norm(p-cep_p) > step_size:
00326 cep_p += unit_vec * step_size
00327 self.set_cep_jtt(arm, cep_p)
00328 rospy.sleep(sleep_time)
00329 self.set_cep_jtt(arm, p)
00330 rospy.sleep(sleep_time)
00331
00332
00333
00334
00335
00336
00337 def get_wrist_force_estimate(self, arm, bias = True, base_frame = False):
00338 if arm != 0:
00339 rospy.logerr('Unsupported arm: %d'%arm)
00340 raise RuntimeError('Unimplemented function')
00341
00342 f = self.r_arm_ftc_estimate.read(without_bias = not bias)
00343 f = f[0:3, :]
00344 if base_frame:
00345 trans, quat = self.tf_lstnr.lookupTransform('/torso_lift_link',
00346 '/ft2_estimate', rospy.Time(0))
00347 rot = tr.quaternion_to_matrix(quat)
00348 f = rot * f
00349 return -f
00350
00351
00352 def get_wrist_force_ati(self, arm, bias = True, base_frame = False):
00353 if arm != 0:
00354 rospy.logerr('Unsupported arm: %d'%arm)
00355 raise RuntimeError('Unimplemented function')
00356
00357 f = self.r_arm_ftc.read(without_bias = not bias)
00358 f = f[0:3, :]
00359 if base_frame:
00360 trans, quat = self.tf_lstnr.lookupTransform('/torso_lift_link',
00361 '/ft2', rospy.Time(0))
00362 rot = tr.quaternion_to_matrix(quat)
00363 f = rot * f
00364 return -f
00365
00366
00367
00368
00369 def get_force_from_torques(self, arm):
00370 if arm != 1:
00371 arm = 0
00372 self.arm_state_lock[arm].acquire()
00373 q = self.arm_angles[arm]
00374 tau = self.arm_efforts[arm]
00375 self.arm_state_lock[arm].release()
00376 p, _ = self.arms.FK_all(arm, q)
00377 J = self.arms.Jacobian(arm, q, p)
00378 f = np.linalg.pinv(J.T) * np.matrix(tau).T
00379 f = f[0:3,:]
00380 return -f
00381
00382
00383 def bias_wrist_ft(self, arm):
00384 if arm != 0:
00385 rospy.logerr('Unsupported arm: %d'%arm)
00386 raise RuntimeError('Unimplemented function')
00387 self.r_arm_ftc.bias()
00388 self.r_arm_ftc_estimate.bias()
00389
00390
00391 def move_gripper(self, arm, amount=0.08, effort = 15):
00392 self.gripper_action_client[arm].send_goal(Pr2GripperCommandGoal(Pr2GripperCommand(position=amount,
00393 max_effort = effort)))
00394
00395
00396
00397 def open_gripper(self, arm):
00398 self.move_gripper(arm, 0.08, -1)
00399
00400
00401
00402 def close_gripper(self, arm, effort = 15):
00403 self.move_gripper(arm, 0.0, effort)
00404
00405
00406
00407
00408 class PR2Arms_kdl():
00409 def __init__(self):
00410 self.right_chain = self.create_right_chain()
00411 fk, ik_v, ik_p, jac = self.create_solvers(self.right_chain)
00412 self.right_fk = fk
00413 self.right_ik_v = ik_v
00414 self.right_ik_p = ik_p
00415 self.right_jac = jac
00416 self.right_tooltip = np.matrix([0.,0.,0.]).T
00417
00418 def create_right_chain(self):
00419 ch = kdl.Chain()
00420 self.right_arm_base_offset_from_torso_lift_link = np.matrix([0., -0.188, 0.]).T
00421
00422 ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotZ),kdl.Frame(kdl.Vector(0.1,0.,0.))))
00423
00424 ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,0.))))
00425
00426 ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.4,0.,0.))))
00427
00428 ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.0,0.,0.))))
00429
00430 ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.321,0.,0.))))
00431
00432 ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,0.))))
00433
00434 ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.,0.,0.))))
00435 return ch
00436
00437 def create_solvers(self, ch):
00438 fk = kdl.ChainFkSolverPos_recursive(ch)
00439 ik_v = kdl.ChainIkSolverVel_pinv(ch)
00440 ik_p = kdl.ChainIkSolverPos_NR(ch, fk, ik_v)
00441 jac = kdl.ChainJntToJacSolver(ch)
00442 return fk, ik_v, ik_p, jac
00443
00444
00445 def set_tooltip(self, arm, p):
00446 if arm == 0:
00447 self.right_tooltip = p
00448 else:
00449 rospy.logerr('Arm %d is not supported.'%(arm))
00450
00451 def FK_kdl(self, arm, q, link_number):
00452 if arm == 0:
00453 fk = self.right_fk
00454 endeffec_frame = kdl.Frame()
00455 kinematics_status = fk.JntToCart(q, endeffec_frame,
00456 link_number)
00457 if kinematics_status >= 0:
00458 return endeffec_frame
00459 else:
00460 rospy.loginfo('Could not compute forward kinematics.')
00461 return None
00462 else:
00463 msg = '%s arm not supported.'%(arm)
00464 rospy.logerr(msg)
00465 raise RuntimeError(msg)
00466
00467
00468 def FK_all(self, arm, q, link_number = 7):
00469 q = self.pr2_to_kdl(q)
00470 frame = self.FK_kdl(arm, q, link_number)
00471 pos = frame.p
00472 pos = ku.kdl_vec_to_np(pos)
00473 pos = pos + self.right_arm_base_offset_from_torso_lift_link
00474 m = frame.M
00475 rot = ku.kdl_rot_to_np(m)
00476 if arm == 0:
00477 tooltip_baseframe = rot * self.right_tooltip
00478 pos += tooltip_baseframe
00479 else:
00480 rospy.logerr('Arm %d is not supported.'%(arm))
00481 return None
00482 return pos, rot
00483
00484 def kdl_to_pr2(self, q):
00485 if q == None:
00486 return None
00487
00488 q_pr2 = [0] * 7
00489 q_pr2[0] = q[0]
00490 q_pr2[1] = q[1]
00491 q_pr2[2] = q[2]
00492 q_pr2[3] = q[3]
00493 q_pr2[4] = q[4]
00494 q_pr2[5] = q[5]
00495 q_pr2[6] = q[6]
00496 return q_pr2
00497
00498 def pr2_to_kdl(self, q):
00499 if q == None:
00500 return None
00501 n = len(q)
00502 q_kdl = kdl.JntArray(n)
00503 for i in range(n):
00504 q_kdl[i] = q[i]
00505 return q_kdl
00506
00507 def Jac_kdl(self, arm, q):
00508 J_kdl = kdl.Jacobian(7)
00509 if arm != 0:
00510 rospy.logerr('Unsupported arm: '+ str(arm))
00511 return None
00512
00513 self.right_jac.JntToJac(q,J_kdl)
00514
00515 kdl_jac = np.matrix([
00516 [J_kdl[0,0],J_kdl[0,1],J_kdl[0,2],J_kdl[0,3],J_kdl[0,4],J_kdl[0,5],J_kdl[0,6]],
00517 [J_kdl[1,0],J_kdl[1,1],J_kdl[1,2],J_kdl[1,3],J_kdl[1,4],J_kdl[1,5],J_kdl[1,6]],
00518 [J_kdl[2,0],J_kdl[2,1],J_kdl[2,2],J_kdl[2,3],J_kdl[2,4],J_kdl[2,5],J_kdl[2,6]],
00519 [J_kdl[3,0],J_kdl[3,1],J_kdl[3,2],J_kdl[3,3],J_kdl[3,4],J_kdl[3,5],J_kdl[3,6]],
00520 [J_kdl[4,0],J_kdl[4,1],J_kdl[4,2],J_kdl[4,3],J_kdl[4,4],J_kdl[4,5],J_kdl[4,6]],
00521 [J_kdl[5,0],J_kdl[5,1],J_kdl[5,2],J_kdl[5,3],J_kdl[5,4],J_kdl[5,5],J_kdl[5,6]],
00522 ])
00523 return kdl_jac
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538 def Jacobian(self, arm, q, pos):
00539 if arm != 0:
00540 rospy.logerr('Arm %d is not supported.'%(arm))
00541 return None
00542
00543 tooltip = self.right_tooltip
00544 self.right_tooltip = np.matrix([0.,0.,0.]).T
00545 v_list = []
00546 w_list = []
00547 for i in range(7):
00548 p, rot = self.FK_all(arm, q, i)
00549 r = pos - p
00550 z_idx = self.right_chain.getSegment(i).getJoint().getType() - 1
00551 z = rot[:, z_idx]
00552 v_list.append(np.matrix(np.cross(z.A1, r.A1)).T)
00553 w_list.append(z)
00554
00555 J = np.row_stack((np.column_stack(v_list), np.column_stack(w_list)))
00556 self.right_tooltip = tooltip
00557 return J
00558
00559 def close_to_singularity(self, arm, q):
00560 pass
00561
00562 def within_joint_limits(self, arm, q, delta_list=[0., 0., 0., 0., 0., 0., 0.]):
00563 if arm == 0:
00564 min_arr = np.radians(np.array([-109., -24, -220, -132, -np.inf, -120, -np.inf]))
00565
00566 max_arr = np.radians(np.array([26., 68, 41, 5, np.inf, 5, np.inf]))
00567 else:
00568 raise RuntimeError('within_joint_limits unimplemented for left arm')
00569
00570 q_arr = np.array(q)
00571 d_arr = np.array(delta_list)
00572 return np.all((q_arr <= max_arr+d_arr, q_arr >= min_arr+d_arr))
00573
00574
00575
00576
00577 if __name__ == '__main__':
00578 from visualization_msgs.msg import Marker
00579 import hrl_lib.viz as hv
00580
00581 rospy.init_node('pr2_arms_test')
00582
00583 pr2_arms = PR2Arms()
00584 pr2_kdl = PR2Arms_kdl()
00585 r_arm, l_arm = 0, 1
00586 arm = r_arm
00587
00588
00589 if True:
00590 np.set_printoptions(precision=2, suppress=True)
00591 while not rospy.is_shutdown():
00592 q = pr2_arms.get_joint_angles(arm)
00593 print 'q in degrees:', np.degrees(q)
00594 rospy.sleep(0.1)
00595
00596 if False:
00597 jep = [0.] * 7
00598 rospy.loginfo('Going to home location.')
00599 raw_input('Hit ENTER to go')
00600 pr2_arms.set_jep(arm, jep, duration=2.)
00601
00602 if False:
00603
00604 marker_pub = rospy.Publisher('/pr2_kdl/ee_marker', Marker)
00605 pr2_kdl.set_tooltip(arm, np.matrix([0.15, 0., 0.]).T)
00606 rt = rospy.Rate(100)
00607 rospy.loginfo('Starting the maker publishing loop.')
00608 while not rospy.is_shutdown():
00609 q = pr2_arms.get_joint_angles(arm)
00610 p, rot = pr2_kdl.FK_all(arm, q)
00611 m = hv.create_frame_marker(p, rot, 0.15, '/torso_lift_link')
00612 m.header.stamp = rospy.Time.now()
00613 marker_pub.publish(m)
00614 rt.sleep()
00615
00616 if False:
00617
00618
00619 while not rospy.is_shutdown():
00620 q = pr2_arms.get_joint_angles(arm)
00621 J_kdl = pr2_kdl.Jac(arm , q)
00622 p, rot = pr2_kdl.FK_all(arm, q)
00623 J_adv = pr2_kdl.Jacobian(arm, q, p)
00624 print J_adv.shape
00625 diff_J = J_kdl - J_adv
00626 print 'difference between KDL and adv is:'
00627 print diff_J
00628 print 'Norm of difference matrix:', np.linalg.norm(diff_J)
00629 raw_input('Move arm into some configuration and hit enter to get the Jacobian.')
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694