def create_position_mps_setup(physicsEngine, graphFileName, dt, max_steps,
ref_frame, checkJointLimits):
def policy_fcn(t: float):
return [
0,
0,
1, # PG position
0,
0, # PG orientation
0.01
] # hand joints
# Set up environment
env = BoxShelvingPosDSSim(
physicsEngine=physicsEngine,
graphFileName=graphFileName,
dt=dt,
max_steps=max_steps,
mps_left=None, # use defaults
mps_right=None, # use defaults
ref_frame=ref_frame,
fixedInitState=True,
collisionConfig={'file': 'collisionModel.xml'},
checkJointLimits=checkJointLimits,
collisionAvoidanceIK=True,
observeVelocities=False,
observeCollisionCost=True,
observePredictedCollisionCost=True,
observeManipulabilityIndex=True,
observeCurrentManipulability=True,
observeDynamicalSystemDiscrepancy=True,
observeTaskSpaceDiscrepancy=True,
observeForceTorque=True,
observeDynamicalSystemGoalDistance=True,
)
print(env.get_body_position('Box', '', ''))
print(env.get_body_position('Box', 'GoalUpperShelve', ''))
print(env.get_body_position('Box', '', 'GoalUpperShelve'))
print(env.get_body_position('Box', 'GoalUpperShelve', 'GoalUpperShelve'))
# Set up policy
policy = TimePolicy(env.spec, policy_fcn, dt)
return env, policy
def create_idle_setup(physicsEngine, graphFileName, dt, max_steps, ref_frame,
checkJointLimits):
# Set up environment
env = BoxShelvingPosDSSim(
physicsEngine=physicsEngine,
graphFileName=graphFileName,
dt=dt,
max_steps=max_steps,
mps_left=None, # use defaults
mps_right=None, # use defaults
ref_frame=ref_frame,
fixedInitState=False,
collisionConfig={'file': 'collisionModel.xml'},
checkJointLimits=checkJointLimits,
)
# Set up policy
policy = IdlePolicy(env.spec) # don't move at all
return env, policy
def default_bs_ds_pos_vx():
return BoxShelvingPosDSSim(
physicsEngine="Vortex",
graphFileName="gBoxShelving_posCtrl.xml", # gBoxShelving_posCtrl.xml or gBoxShelving_trqCtrl.xml
dt=1 / 100.0,
max_steps=2000,
fixed_init_state=True,
ref_frame="upperGoal",
tasks_left=None,
tasks_right=None,
collisionConfig={"file": "collisionModel.xml"},
taskCombinationMethod="sum",
checkJointLimits=True,
collisionAvoidanceIK=True,
observeVelocities=True,
observeForceTorque=True,
observeCollisionCost=True,
observePredictedCollisionCost=True,
observeManipulabilityIndex=True,
observeTaskSpaceDiscrepancy=True,
observeDynamicalSystemDiscrepancy=True,
observeDynamicalSystemGoalDistance=True,
)
def default_bs_pos_bt():
return BoxShelvingPosDSSim(
physicsEngine='Bullet',
graphFileName=
'gBoxShelving_posCtrl.xml', # gBoxShelving_posCtrl.xml or gBoxShelving_trqCtrl.xml
dt=1 / 100.,
max_steps=2000,
fix_init_state=True,
ref_frame='upperGoal',
mps_left=None,
mps_right=None,
collisionConfig={'file': 'collisionModel.xml'},
taskCombinationMethod='sum',
checkJointLimits=True,
collisionAvoidanceIK=True,
observeVelocities=True,
observeForceTorque=True,
observeCollisionCost=True,
observePredictedCollisionCost=True,
observeManipulabilityIndex=True,
observeTaskSpaceDiscrepancy=True,
observeDynamicalSystemDiscrepancy=True,
observeDynamicalSystemGoalDistance=True,
)