本文整理汇总了Python中World.World.transform方法的典型用法代码示例。如果您正苦于以下问题：Python World.transform方法的具体用法？Python World.transform怎么用？Python World.transform使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类World.World的用法示例。
在下文中一共展示了World.transform方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: _get_arm_states
# 需要导入模块: from World import World [as 别名]
# 或者: from World.World import transform [as 别名]
def _get_arm_states(self):
'''Returns the current arms states in the right format'''
abs_ee_poses = [Arms.get_ee_state(0),
Arms.get_ee_state(1)]
joint_poses = [Arms.get_joint_state(0),
Arms.get_joint_state(1)]
states = [None, None]
for arm_index in [0, 1]:
nearest_obj = self.world.get_nearest_object(
abs_ee_poses[arm_index])
if (nearest_obj == None):
states[arm_index] = ArmState(ArmState.ROBOT_BASE,
abs_ee_poses[arm_index],
joint_poses[arm_index], Object())
else:
# Relative
rel_ee_pose = World.transform(
abs_ee_poses[arm_index],
'base_link', nearest_obj.name)
states[arm_index] = ArmState(ArmState.OBJECT,
rel_ee_pose,
joint_poses[arm_index], nearest_obj)
return states示例2: solve_ik_for_arm
# 需要导入模块: from World import World [as 别名]
# 或者: from World.World import transform [as 别名]
def solve_ik_for_arm(arm_index, arm_state, cur_arm_pose=None):
'''Finds an IK solution for a particular arm pose'''
# We need to find IK only if the frame is relative to an object
if (arm_state.refFrame == ArmState.OBJECT):
#rospy.loginfo('solve_ik_for_arm: Arm ' + str(arm_index) + ' is relative')
solution = ArmState()
target_pose = World.transform(arm_state.ee_pose,
arm_state.refFrameObject.name, 'base_link')
target_joints = Arms.arms[arm_index].get_ik_for_ee(target_pose,
arm_state.joint_pose)
if (target_joints == None):
rospy.logerr('No IK for relative end-effector pose.')
return solution, False
else:
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = Pose(target_pose.position,
target_pose.orientation)
solution.joint_pose = target_joints
return solution, True
elif (arm_state.refFrame == ArmState.ROBOT_BASE):
#rospy.loginfo('solve_ik_for_arm: Arm ' + str(arm_index) + ' is absolute')
target_joints = Arms.arms[arm_index].get_ik_for_ee(
arm_state.ee_pose,
arm_state.joint_pose)
if (target_joints == None):
rospy.logerr('No IK for absolute end-effector pose.')
return arm_state, False
else:
solution = ArmState()
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation)
solution.joint_pose = target_joints
return solution, True
elif (arm_state.refFrame == ArmState.PREVIOUS_POSE):
#Calculate new arm state offset based on previous arm state
solution = ArmState()
target_ee_pose = Pose(Point(
cur_arm_pose.position.x + arm_state.ee_pose.position.x,
cur_arm_pose.position.y + arm_state.ee_pose.position.y,
cur_arm_pose.position.z + arm_state.ee_pose.position.z),
Quaternion(cur_arm_pose.orientation.x,
cur_arm_pose.orientation.y,
cur_arm_pose.orientation.z,
cur_arm_pose.orientation.w))
target_joints = Arms.arms[arm_index].get_ik_for_ee(target_ee_pose,
arm_state.joint_pose)
if (target_joints == None):
rospy.logerr('No IK for relative end-effector pose.')
return solution, False
else:
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = target_ee_pose
solution.joint_pose = target_joints
return solution, True
else:
return arm_state, True