def editExistingGrasp(item,object,gindex):
global world,robot
grasp = None
if isinstance(gindex,int):
grasp = item.grasps[gindex]
else:
for g in item.grasps:
if g.name == gindex:
grasp = g
break
if grasp == None:
print "Grasp",gindex,"does not exist, creating new grasp"
raise RuntimeError("Invalid grasp")
#get the object transform
object.setTransform(*se3.inv(grasp.grasp_xform))
open_config = grippermodule.commandToConfig(grasp.gripper_open_command)
closed_config = grippermodule.commandToConfig(grasp.gripper_close_command)
robot.setConfig(closed_config)
xform = resource.get(name=None,type='RigidTransform',default=object.getTransform(),description="Transform of "+item.name,world=world,frame=object)
if xform==None: return None
#set the object in the world
object.setTransform(*xform)
#get the hand open configuration
open_config = resource.get(name=None,type='Config',default=open_config,description="Hand open configuration",world=world)
if open_config==None: return None
closed_config = resource.get(name=None,type='Config',default=closed_config,description="Hand closed configuration",world=world)
if closed_config==None: return None
#return grasp
grasp.grasp_xform = se3.inv(xform)
grasp.gripper_close_command = grippermodule.configToCommand(closed_config)
grasp.gripper_open_command = grippermodule.configToCommand(open_config)
return grasp
def editNewGrasp(item,object,name=None,openClose=1):
global world,robot
global default_open_config,default_close_config
#get the object transform
xform = resource.get(name=None,
type='RigidTransform',
default=object.getTransform(),
description="Transform of "+item.name,
world=world,frame=object)
if xform==None: return None
#set the object in the world
object.setTransform(*xform)
if openClose:
#get the hand open configuration
open_config = resource.get(name=None,type='Config',default=default_open_config,description="Hand open configuration",world=world)
if open_config==None: return None
closed_config = resource.get(name=None,type='Config',default=open_config,description="Hand closed configuration",world=world)
if closed_config==None: return None
#return grasp
grasp = apc.ItemGrasp()
grasp.grasp_xform = se3.inv(xform)
if openClose:
grasp.gripper_close_command = grippermodule.configToCommand(closed_config)
grasp.gripper_open_command = grippermodule.configToCommand(open_config)
if name==None:
#default: just number it
grasp.name = str(len(item.grasps))
else:
grasp.name = name
return grasp
def __init__(self, configFile, viconFile):
self._viconFile = viconFile
self._configFile = configFile
self._viconMarkers = utils.parseCSVToMarkers(viconFile)
configfn = configFile.split('/')[-1]
configDir = '/'.join(configFile.split('/')[:-1])
self._gloveConfigs = resource.get(configfn, directory=configDir)
self.data = self._makeData()
def localizeOrderBin(self):
order_bin_xform = (so3.identity(),[0.57,0,-0.87])
resource.setDirectory("resources/")
return resource.get("calibrated_order_bin.xform",type="RigidTransform",default=order_bin_xform,doedit=False)
T = numpy.array([[ 1, 0, 0, 0.57 ],
[ 0, 1, 0, 0 ],
[ 0, 0, 1, -0.87 ],
[ 0, 0, 0, 1 ]])
return (list(T[:3, :3].T.flat), list(T[:3, 3].flat))
def localizeShelf(self):
#distance away
shelf_xform = ([0,1,0,-1,0,0,0,0,1],[1.1,-0.055,-.9])
resource.setDirectory("resources/")
return resource.get("calibrated_shelf.xform",type="RigidTransform",default=shelf_xform,doedit=False)
T = numpy.array([[ 0, -1, 0, 1.1 ], # distance from front of robot
[ 1, 0, 0, -0.055 ], # left/right
[ 0, 0, 1, -0.9 ], # height
[ 0, 0, 0, 1 ]])
return (list(T[:3, :3].T.flat), list(T[:3, 3].flat))
left_arm_geometry_indices = [15,16,17,18,19,21,22,23,30,31]
right_arm_geometry_indices = [35,36,37,38,39,41,42,43,50,51]
left_hand_link_start = 54
right_hand_link_start = 54+gripper.numDofs
left_hand_geometry_indices = range(54,54+gripper.numDofs)
right_hand_geometry_indices = range(54+gripper.numDofs,54+gripper.numDofs*2)
#indices of the left and right arms in the Baxter robot file
left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
#local transformations (rotation, translation pairs) of the grasp center
resource.setDirectory("resources/reflex")
left_gripper_center_xform = resource.get("left_gripper_center.xform",
type='RigidTransform',
description="Left gripper center",
world="klampt_models/"+klampt_model_name,
frame=left_gripper_link_name)
right_gripper_center_xform = resource.get("right_gripper_center.xform",
type='RigidTransform',
default=left_gripper_center_xform,
description="Right gripper center",
world="klampt_models/"+klampt_model_name,
frame=right_gripper_link_name)
#resting configuration
rest_config = resource.get("baxter_rest.config",type="Config",description="Rest configuration",world="klampt_models/"+klampt_model_name)
def set_model_gripper_command(robot,limb,command):
"""Given the Baxter RobotModel 'robot' at its current configuration,
this will set the configuration so the gripper on limb 'limb' is
world = WorldModel()
world.readFile('../ece490-s2016/apc2015/klampt_models/apc.xml')
#sets the world to the state depicted in the file
obj = ik.objective(world.robot(0).link(22),local=[0,0,0],world=[1,0,1])
visualization.add("world",world)
#visualization.add("ik objective",obj)
q0 = world.robot(0).getConfig()
#copy vector of world robot config
q1 = q0[:]
#q1[16] = 1
#q2 = q1[:]
#q2[16] = 0
#path = [q0,q1,q2]
q1 = resource.get("goal.config",description="Goal config for left arm",doedit=True,default=q1,editor='visual',world=world)
#robot links under suspicion?
subset = [15,16,17,18,19,20,21,22]
collider = robotcollide.WorldCollider(world)
#probably want to ignore collisions beteween other arm/links and the world to make things faster...
space = robotcspace.RobotSubsetCSpace(world.robot(0),subset,collider)
planner = cspace.MotionPlan(space, "rrt*")
#extract out cspace configurations
start = [q0[i] for i in subset]
goal = [q1[i] for i in subset]
print "Goal config",goal
planner.setEndpoints(start,goal)
for iters in xrange(10000):
planner.planMore(1)
qmin,qmax = robot.getJointLimits()
numObs = 10
jointEncoderError = 1e-5
sensorErrorRads = 1e-2
sensorErrorMeters = 2e-3
trueCalibrationConfigs = []
calibrationConfigs = []
trueObservations = []
observations = []
for obs in xrange(numObs):
q0 = [random.uniform(a,b) for (a,b) in zip(qmin,qmax)]
#don't move head
for i in range(13):
q0[i] = 0
trueCalibrationConfigs.append(q0)
trueCalibrationConfigs=resource.get("calibration.configs",default=trueCalibrationConfigs,type="Configs",description="Calibration configurations",world=world)
for q0 in trueCalibrationConfigs:
robot.setConfig(q0)
obs0 = se3.mul(se3.inv(lc.getTransform()),lm.getTransform())
dq = [random.uniform(-jointEncoderError,jointEncoderError) for i in range(len(q0))]
dobs = (so3.from_moment([random.uniform(-sensorErrorRads,sensorErrorRads) for i in range(3)]),[random.uniform(-sensorErrorMeters,sensorErrorMeters) for i in range(3)])
calibrationConfigs.append(vectorops.add(q0,dq))
observations.append(se3.mul(obs0,dobs))
trueObservations.append(obs0)
if DO_VISUALIZATION:
rgroup = coordinates.addGroup("calibration ground truth")
rgroup.addFrame("camera link",worldCoordinates=pc.getTransform())
rgroup.addFrame("marker link",worldCoordinates=pm.getTransform())
rgroup.addFrame("camera (ground truth)",parent="camera link",relativeCoordinates=Tc0)
rgroup.addFrame("marker (ground truth)",parent="marker link",relativeCoordinates=Tm0)
#load the items and spawn one in the world
apc.load_item(objectname,gripper=grippername)
item = apc.item_info[objectname]
item.geometry = apc.load_item_geometry(item)
itemposed = apc.ItemInBin(item,'blah')
itemposed.xform = se3.identity()
apc.spawn_item_in_world(itemposed,world)
#set the directory
print "Resouce directory is",os.path.join('../resources/',os.path.splitext(grippermodule.klampt_model_name)[0])
resource.setDirectory(os.path.join('../resources/',os.path.splitext(grippermodule.klampt_model_name)[0]))
#edit the default configurations
default_open_config = resource.get('default_open.config',
description='Default hand open configuration',
doedit='auto',
world=world)
default_closed_config = resource.get('default_closed.config',
description='Default hand closed configuration',
doedit='auto',
world=world)
def editNewGrasp(item,object,name=None,openClose=1):
global world,robot
global default_open_config,default_close_config
#get the object transform
xform = resource.get(name=None,
type='RigidTransform',
default=object.getTransform(),
description="Transform of "+item.name,
print """resourcetest.py: This program gives an example of how to use the
resource module."""
worldfile = "../../data/athlete_plane.xml"
robotname = 'athlete'
world = WorldModel()
world.readFile(worldfile)
#look in resources/athlete/
resource.setDirectory('resources/' + robotname)
config = resource.get("resourcetest1.config",
description="First config, always edited",
doedit=True,
editor='visual',
world=world)
print "Config 1:", config
config = resource.get("resourcetest1.config",
description="Trying this again...",
editor='visual',
world=world)
print "Config 2:", config
config = resource.get("resourcetest2.config",
description="Another configuration",
editor='visual',
world=world)
print "Config 3:", config
qmin,qmax = robot.getJointLimits()
numObs = 10
jointEncoderError = 1e-5
sensorErrorRads = 1e-2
sensorErrorMeters = 2e-3
trueCalibrationConfigs = []
calibrationConfigs = []
trueObservations = []
observations = []
for obs in xrange(numObs):
q0 = [random.uniform(a,b) for (a,b) in zip(qmin,qmax)]
#don't move head
for i in range(13):
q0[i] = 0
trueCalibrationConfigs.append(q0)
trueCalibrationConfigs=resource.get("calibration.configs",default=trueCalibrationConfigs,type="Configs",description="Calibration configurations",world=world)
for q0 in trueCalibrationConfigs:
robot.setConfig(q0)
obs0 = se3.mul(se3.inv(lc.getTransform()),lm.getTransform())
dq = [random.uniform(-jointEncoderError,jointEncoderError) for i in range(len(q0))]
dobs = (so3.from_moment([random.uniform(-sensorErrorRads,sensorErrorRads) for i in range(3)]),[random.uniform(-sensorErrorMeters,sensorErrorMeters) for i in range(3)])
calibrationConfigs.append(vectorops.add(q0,dq))
observations.append(se3.mul(obs0,dobs))
trueObservations.append(obs0)
if DO_VISUALIZATION:
rgroup = coordinates.addGroup("calibration ground truth")
rgroup.addFrame("camera link",worldCoordinates=pc.getTransform())
rgroup.addFrame("marker link",worldCoordinates=pm.getTransform())
rgroup.addFrame("camera (ground truth)",parent="camera link",relativeCoordinates=Tc0)
rgroup.addFrame("marker (ground truth)",parent="marker link",relativeCoordinates=Tm0)
from klampt import resource
from klampt import *
print """resourcetest.py: This program gives an example of how to use the
resource module."""
worldfile = "../../data/athlete_plane.xml"
robotname = 'athlete'
world = WorldModel()
world.readFile(worldfile)
#look in resources/athlete/
resource.setDirectory('resources/'+robotname)
config = resource.get("resourcetest1.config",description="First config, always edited",doedit=True,editor='visual',world=world)
print "Config 1:",config
config = resource.get("resourcetest1.config",description="Trying this again...",editor='visual',world=world)
print "Config 2:",config
config = resource.get("resourcetest2.config",description="Another configuration",editor='visual',world=world)
print "Config 3:",config
if config != None:
config[3] += 1.0
resource.set("resourcetest3_high.config",config)
world.robot(0).setConfig(config)
#testing transform editor
xform = resource.get(name=None,type='RigidTransform',frame=world.robot(0).getLink(5).getTransform(),world=world)
q = world.robot(0).getConfig()
q[9] = -1.0
world.robot(0).setConfig(q)
visualization.unlock()
time.sleep(1.0)
if not visualization.shown():
break
print "Hiding visualization window"
visualization.show(False)
"""
# look in resources/athlete/
resource.setDirectory("resources/" + robotname)
config1 = resource.get(
"resourcetest1.config", description="First config, always edited", doedit=True, editor="visual", world=world
)
print "Config 1:", config1
config2 = resource.get("resourcetest1.config", description="Trying this again...", editor="visual", world=world)
print "Config 2:", config2
config3 = resource.get("resourcetest2.config", description="Another configuration", editor="visual", world=world)
print "Config 3:", config3
if config3 != None:
config3hi = config3[:]
config3hi[3] += 1.0
resource.set("resourcetest3_high.config", config3hi)
world.robot(0).setConfig(config3hi)
configs = []
left_arm_link_names = [
'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
'left_wrist'
]
right_arm_link_names = [
'right_upper_shoulder', 'right_lower_shoulder', 'right_upper_elbow',
'right_lower_elbow', 'right_upper_forearm', 'right_lower_forearm',
'right_wrist'
]
#local transformations (rotation, translation pairs) of the grasp center
resource.setDirectory("resources/reflex")
left_gripper_center_xform = resource.get("left_gripper_center.xform",
type='RigidTransform',
description="Left gripper center",
world="klampt_models/" +
klampt_model_name,
frame=left_gripper_link_name)
right_gripper_center_xform = resource.get("right_gripper_center.xform",
type='RigidTransform',
default=left_gripper_center_xform,
description="Right gripper center",
world="klampt_models/" +
klampt_model_name,
frame=right_gripper_link_name)
#resting configuration
rest_config = resource.get("baxter_rest.config",
type="Config",
description="Rest configuration",
world="klampt_models/" + klampt_model_name)
