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)
