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