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
def _ravenStateCallback(self,msg): 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)