Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Bugfix/get manipulator index #326

Merged
merged 2 commits into from
Jul 24, 2016
Merged

Bugfix/get manipulator index #326

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7ac6d39
Replaced get_manip_idx with GetManipulatorIndex.
Jul 23, 2016
84435c0
Suppress warning in GetManipulatorIndex.
Jul 23, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 13 additions & 15 deletions src/prpy/tsr/generic.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,21 +1,19 @@
import numpy
import warnings
from prpy.tsr.tsrlibrary import TSRFactory
from prpy.tsr.tsr import TSR, TSRChain
from ..util import GetManipulatorIndex

def get_manip_idx(robot, manip=None):
"""
Helper function for getting the manipulator index
to be used by the TSR
Helper function for getting the manipulator index to be used by the TSR.

Deprecated. Please use util.GetActiveManipulatorIndex instead.
"""
if manip is None:
manip_idx = robot.GetActiveManipulatorIndex()
else:
import openravepy
p = openravepy.KinBody.SaveParameters
with robot.CreateRobotStateSaver(p.ActiveManipulator):
robot.SetActiveManipulator(manip)
manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
return manip_idx
warnings.warn(
'tsr.get_manip_idx is deprecated. Please use'
' util.GetActiveManipulatorIndex instead.', DeprecationWarning)
return GetActiveManipulatorIndex(robot, manip)

def cylinder_grasp(robot, obj, obj_radius, obj_height,
lateral_offset = 0.0,
Expand Down Expand Up @@ -60,7 +58,7 @@ def cylinder_grasp(robot, obj, obj_radius, obj_height,
'than or equal to the second (current values [%f, %f])'
% (yaw_range[0], yaw_range[1]))

manip_idx = get_manip_idx(robot, manip=manip)
manip_idx = GetManipulatorIndex(robot, manip=manip)

T0_w = obj.GetTransform()
total_offset = lateral_offset + obj_radius
Expand Down Expand Up @@ -135,7 +133,7 @@ def box_grasp(robot, box, length, width, height,
if lateral_tolerance < 0.0:
raise Exception('lateral_tolerance must be >= 0.0')

manip_idx = get_manip_idx(robot, manip=manip)
manip_idx = GetManipulatorIndex(robot, manip=manip)

T0_w = box.GetTransform()

Expand Down Expand Up @@ -251,7 +249,7 @@ def place_object(robot, obj, pose_tsr_chain, manip=None,
manipulator of the robot is used
"""

manip_idx = get_manip_idx(robot, manip=manip)
manip_idx = GetManipulatorIndex(robot, manip=manip)
if manip is None:
manip = robot.GetManipulators()[manip_idx]

Expand Down Expand Up @@ -300,7 +298,7 @@ def transport_upright(robot, obj,
if yaw_epsilon < 0.0:
raise Exception('yaw_epsilon must be >= 0')

manip_idx = get_manip_idx(robot, manip=manip)
manip_idx = GetManipulatorIndex(robot, manip=manip)
if manip is None:
manip = robot.GetManipulators()[manip_idx]

Expand Down
11 changes: 10 additions & 1 deletion src/prpy/util.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@
import scipy.optimize
import threading
import time
import warnings


logger = logging.getLogger(__name__)
Expand Down Expand Up @@ -2054,6 +2055,7 @@ def GetManipulatorIndex(robot, manip=None):
@param manip The robot manipulator
@return (manip, manip_idx) The manipulator and its index
"""
from openravepy import DebugLevel, RaveGetDebugLevel, RaveSetDebugLevel

with robot.GetEnv():
if manip is None:
Expand All @@ -2062,7 +2064,14 @@ def GetManipulatorIndex(robot, manip=None):
with robot.CreateRobotStateSaver(
robot.SaveParameters.ActiveManipulator):
robot.SetActiveManipulator(manip)
manip_idx = manip.GetRobot().GetActiveManipulatorIndex()

# Ignore GetActiveManipulatorIndex's DeprecationWarning.
debug_level = RaveGetDebugLevel()
try:
RaveSetDebugLevel(DebugLevel.Error)
manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
finally:
RaveSetDebugLevel(debug_level)

return (manip, manip_idx)

Expand Down