def _ravenStateCallback(self,msg):
if self.errorCorrectionMode == 'NONE':
for arm in self.arms:
if self.estimatedPose.has_key(arm):
if tfx.stamp(msg.header.stamp) - self.estimatedPose[arm][0].stamp < 0.2:
return
arm_msg = [msg_arm for msg_arm in msg.arms if msg_arm.name == arm][0]
joints = dict((j.type,j.position) for j in arm_msg.joints)
fwdArmKinPose, grasp = kinematics.fwdArmKin(arm,joints,stamp=msg.header.stamp)
estPose = tfx.pose(fwdArmKinPose,frame='0_link',stamp = msg.header.stamp)
estPose.frame = '0_link' # TEMP
self.estimatedPose[arm] = (estPose,False)
self.est_pose_pub[arm].publish(estPose.msg.PoseStamped())
elif self.errorCorrectionMode == 'SYS':
for arm in self.arms:
if self.estimatedPose.has_key(arm):
if tfx.stamp(msg.header.stamp) - self.estimatedPose[arm][0].stamp < 0.2:
return
arm_msg = [msg_arm for msg_arm in msg.arms if msg_arm.name == arm][0]
joints = dict((j.type,j.position) for j in arm_msg.joints)
fwdArmKinPose, grasp = kinematics.fwdArmKin(arm,joints,stamp=msg.header.stamp)
estPose = tfx.pose(self.sys_error[arm] * fwdArmKinPose,frame='0_link',stamp = msg.header.stamp)
estPose.frame = '0_link' # TEMP
self.estimatedPose[arm] = (estPose,False)
self.est_pose_pub[arm].publish(estPose.msg.PoseStamped())
elif self.errorCorrectionMode == 'PRE_POST':
if self.calcPose:
prevTime = self.calcPose.values()[0].stamp
if tfx.stamp(msg.header.stamp).seconds - prevTime.seconds < 0.2:
return
for arm in self.arms:
arm_msg = [msg_arm for msg_arm in msg.arms if msg_arm.name == arm][0]
#self.calcPose[arm] = tfx.pose(arm_msg.tool.pose,header=msg.header)
joints = dict((j.type,j.position) for j in arm_msg.joints)
fwdArmKinPose, grasp = kinematics.fwdArmKin(arm,joints)
self.calcPose[arm] = (fwdArmKinPose.as_tf() * self.calcPosePostAdjustment[arm]).as_pose(stamp=msg.header.stamp)
self._updateEstimatedPose(arm)
def jointDictsToPoses(self, armName, jointTrajDicts):
"""
Converts a list of joint trajectory dicts
to a list of poses using openrave FK
"""
poseList = []
for jointDict in jointTrajDicts:
pose, grasp = kin.fwdArmKin(armName, jointDict)
poseList.append(pose)
return poseList
