def run_pickled_demo(fname):
envfile, start_config, goal_config, basetrajs1, armtrajs1, basetrajs2, armtrajs2, assembly, assembly_pose, robots, part_responsibilities, robot_start_poses = load_pickled_demo(
fname)
youbotenv = youbotpy.YoubotEnv(sim=True,viewer=True,env_xml=envfile, \
youbot_names=robots)
env = youbotenv.env
youbots = youbotenv.youbots
for y in basetrajs1:
btraj = orpy.RaveCreateTrajectory(env, "")
btraj.deserialize(basetrajs1[y])
basetrajs1[y] = btraj
atraj = orpy.RaveCreateTrajectory(env, "")
atraj.deserialize(armtrajs1[y])
armtrajs1[y] = atraj
for y in basetrajs2:
btraj = orpy.RaveCreateTrajectory(env, "")
btraj.deserialize(basetrajs2[y])
basetrajs2[y] = btraj
atraj = orpy.RaveCreateTrajectory(env, "")
atraj.deserialize(armtrajs2[y])
armtrajs2[y] = atraj
for name in youbots:
youbots[name].SetTransform(robot_start_poses[name])
youbotenv.MoveGripper(name, 0.01, 0.01) # open grippers
raw_input('Hit Enter to run.')
motionplanner = MultiYoubotPlanner.MultiYoubotPlanner(env, youbots)
motionplanner.Execute(basetrajs1, armtrajs1)
for obj in assembly.object_names:
youbots[part_responsibilities[obj]].Grab(env.GetKinBody(obj))
motionplanner.Execute(basetrajs2, armtrajs2)
raw_input('Hit Enter to destroy environment.')
youbotenv.env.Destroy()
orpy.RaveDestroy()
import youbotpy
from youbotpy import youbotik as yik
import GraspGenerator
#import Assembly
from AssemblyOperation import PartGrasp, Part
import AssemblyGraspCSP
import CollisionConstraints
import BusRide
from chairenv import *
#from pictureenv import *
#from wingdemoenv import *
youbotenv = youbotpy.YoubotEnv(sim=True,viewer=True,env_xml=envfile, \
youbot_names=all_robot_names)
env = youbotenv.env
youbots = youbotenv.youbots
for name in youbots:
youbots[name].SetTransform(robot_start_poses[name])
youbotenv.MoveGripper(name, 0.01, 0.01) # open grippers
yik.init(youbots[youbots.keys()[0]]) # does not matter which robot
# we use to initialize since
# all youbots have same kinematics.
grasp_generator = GraspGenerator.GraspGenerator(env)
for part in all_parts:
part.body = env.GetKinBody(part.name)
part.SetGrasps(grasp_generator.GeneratePartGrasps(part))
BACKUP_AMT = 0.03
LIFT_AMT = 0.3
MOVE_AMT = 0.1
start_var_name = None
#start_var_name = 'seat__seat__0'
use_pickled_plan = True
#pickled_plan_file_name = 'corrected_velcro.p'
pickled_plan_file_name = 'tableplan.p'
sim = False
use_vicon_for_start_poses = False
use_boots = False
youbotenv = youbotpy.YoubotEnv(sim=sim,viewer=True,env_xml=envfile, \
youbot_names=all_robot_names, registered_objects=[])
env = youbotenv.env
youbots = youbotenv.youbots
if sim:
for name in youbots:
youbots[name].SetTransform(robot_start_poses[name])
youbotenv.MoveGripper(name, 0.01, 0.01) # open grippers
yik.init(youbots[youbots.keys()[0]]) # does not matter which robot
# we use to initialize since
# all youbots have same kinematics.
grasp_generator = GraspGenerator.GraspGenerator(env)
pickle_plan = []
if use_vicon_for_start_poses:
sim = False
if sim:
THRESH = 0.004
arm_names = []
for i in range(5):
arm_names.append('arm_joint_' + str(i + 1))
unit = 's^-1 rad'
all_robot_names = ['drc1']
r = all_robot_names[0]
envfile = '/home/drl-mocap/git_scratch/3DoodlerController/drl-youbot-openrave/models/environments/floor.env.xml'
youbotenv = youbotpy.YoubotEnv(sim=sim,viewer=True,env_xml=envfile, \
youbot_names=all_robot_names, registered_objects=None, youbot_model='kuka-youbot.robot.xml')
env = youbotenv.env
youbots = youbotenv.youbots
if sim:
for name in youbots:
youbotenv.MoveGripper(name,0.01,0.01) # open grippers
yik.init(youbots[youbots.keys()[0]]) # does not matter which robot
# we use to initialize since
# all youbots have same kinematics.
offset = np.array([2.950, 1.1345, -2.5482, 1.7890, 2.9234])
robot = youbots['drc1']
import numpy as np
import time
import sys
import pickle
import random
import copy
import IPython
from itertools import izip, product, combinations, chain
import openravepy as orpy
import youbotpy
from youbotpy import youbotik as yik
simulation = False
robotnames = ['drc1', 'drc3']
youbotenv = youbotpy.YoubotEnv(sim=simulation,viewer=True,env_xml='environments/espresso.env.xml', \
youbot_names=robotnames)
env = youbotenv.env
youbots = youbotenv.youbots
yik.init(youbots[youbots.keys()[0]]) # does not matter which robot
# we use to initialize since
# all youbots have same kinematics.
if simulation:
drc1_pose = np.eye(4)
drc1_pose[:2, 3] = drc1_pose[:2, 3] + np.array([-1.0, 1.0])
drc3_pose = np.eye(4)
drc3_pose[:2, 3] = drc3_pose[:2, 3] + np.array([-1.0, -1.0])
youbots['drc1'].SetTransform(drc1_pose)
youbots['drc3'].SetTransform(drc3_pose)
import numpy as np
import time
import sys
import pickle
import random
import copy
import IPython
from itertools import izip, product, combinations, chain
import openravepy as orpy
import youbotpy
from youbotpy import youbotik as yik
robotnames = ['drc1', 'drc2', 'drc3']
youbotenv = youbotpy.YoubotEnv(sim=True,viewer=True,env_xml='environments/simple.env.xml', \
youbot_names=robotnames)
env = youbotenv.env
youbots = youbotenv.youbots
yik.init(youbots[youbots.keys()[0]]) # does not matter which robot
# we use to initialize since
# all youbots have same kinematics.
basemanips = {}
for name in robotnames:
basemanips[name] = orpy.interfaces.BaseManipulation(youbots[name],
plannername='BiRRT')
def MoveArmTo(robot, goal, planner):
with env:
use_pickled_plan = False
>>>>>>> .r1790
#pickled_plan_file_name = 'fullchair_mod.p'
<<<<<<< .mine
#pickled_plan_file_name = 'corrected.p'
pickled_plan_file_name = 'corrected_velcro.p'
=======
#pickled_plan_file_name = 'corrected.p'
pickled_plan_file_name = 'tableplan.p'
>>>>>>> .r1790
#pickled_plan_file_name = 'chair_env3.p'
sim = False
use_vicon_for_start_poses = True
use_boots = False
youbotenv = youbotpy.YoubotEnv(sim=sim,viewer=True,env_xml=envfile, \
youbot_names=all_robot_names, registered_objects=[o for o in all_object_names if not ('fastener' in o or 'leftside' in o or 'cseat' in o)])
env = youbotenv.env
youbots = youbotenv.youbots
if sim:
for name in youbots:
youbots[name].SetTransform(robot_start_poses[name])
youbotenv.MoveGripper(name,0.01,0.01) # open grippers
yik.init(youbots[youbots.keys()[0]]) # does not matter which robot
# we use to initialize since
# all youbots have same kinematics.
grasp_generator = GraspGenerator.GraspGenerator(env)
pickle_plan = []
if use_vicon_for_start_poses: