def setUp(self):
self.q0 = [0, 0, 0, -1.75, 0, 1.0, 0]
self.time_step = 2e-3
self.prevdir = os.getcwd()
os.chdir(os.path.expanduser("pddl_planning"))
task, diagram, state_machine = load_dope(time_step=self.time_step,
dope_path="poses.txt",
goal_name="soup",
is_visualizing=False)
plant = task.mbp
task.publish()
context = diagram.GetMutableSubsystemContext(plant, task.diagram_context)
world_frame = plant.world_frame()
tree = plant.tree()
X_WSoup = tree.CalcRelativeTransform(
context, frame_A=world_frame, frame_B=plant.GetFrameByName("base_link_soup"))
self.manip_station_sim = ManipulationStationSimulator(
time_step=self.time_step,
object_file_path="./models/ycb_objects/soup_can.sdf",
object_base_link_name="base_link_soup",
X_WObject=X_WSoup)
def test_open_door_by_impedance_and_position(self):
modes = ("Impedance", "Position")
is_plan_runner_diagram_list = [False, True]
for is_plan_runner_diagram in is_plan_runner_diagram_list:
for mode in modes:
# Create simulator
manip_station_sim = ManipulationStationSimulator(
time_step=2e-3)
plan_list, gripper_setpoint_list = \
GenerateOpenLeftDoorPlansByImpedanceOrPosition(
open_door_method=mode, is_open_fully=True)
# Make a copy of the initial position of the plans passed to PlanRunner.
q_iiwa_beginning = plan_list[0].traj.value(0).flatten()
iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, \
plant_state_log, t_plan = manip_station_sim.RunSimulation(
plan_list, gripper_setpoint_list,
extra_time=2.0, real_time_rate=0.0, q0_kuka=self.q0, is_visualizing=False,
is_plan_runner_diagram=is_plan_runner_diagram)
# Run tests
self.InspectLog(plant_state_log, manip_station_sim.plant)
self.assertTrue(
self.HasReturnedToQtarget(q_iiwa_beginning,
plant_state_log,
manip_station_sim.plant))
def test_open_door_by_trajectory(self):
# Create simulator and run simulation
manip_station_sim = ManipulationStationSimulator(time_step=2e-3)
plan_list, gripper_setpoint_list = \
GenerateOpenLeftDoorPlansByTrajectory()
iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, \
plant_state_log = manip_station_sim.RunSimulation(
plan_list, gripper_setpoint_list,
extra_time=2.0, real_time_rate=0.0, q0_kuka=self.q0, is_visualizing=False)
# Run tests
self.InspectLog(plant_state_log, manip_station_sim.plant)
def test_open_door_by_trajectory(self):
is_plan_runner_diagram_list = [True, False]
for is_plan_runner_diagram in is_plan_runner_diagram_list:
plan_list, gripper_setpoint_list = \
GenerateOpenLeftDoorPlansByTrajectory()
# Create simulator
manip_station_sim = ManipulationStationSimulator(time_step=2e-3)
iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, \
plant_state_log, t_plan = manip_station_sim.RunSimulation(
plan_list, gripper_setpoint_list,
extra_time=2.0, real_time_rate=0.0, q0_kuka=self.q0, is_visualizing=False,
is_plan_runner_diagram=is_plan_runner_diagram)
# Run tests
self.InspectLog(plant_state_log, manip_station_sim.plant)
print "Estimated right door angle: " + str(estimated_right_door_angle)
left_door_angle = -estimated_left_door_angle
rotation_matrix = np.array([[np.cos(left_door_angle), -np.sin(left_door_angle), 0], [np.sin(left_door_angle), np.cos(left_door_angle), 0], [0, 0, 1]])
p_handle_2_hinge_new = np.dot(rotation_matrix, p_handle_2_hinge)
p_LC_handle_new = p_handle_2_hinge_new + p_LC_left_hinge
p_WC_handle_new = p_WL + p_LC_handle_new
theta0_hinge_new = np.arctan2(np.abs(p_handle_2_hinge_new[0]),
np.abs(p_handle_2_hinge_new[1])) # = theta0_hinge - left_door_angle
# Construct simulator system.
object_file_path = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
manip_station_sim = ManipulationStationSimulator(
time_step=2e-3,
object_file_path=object_file_path,
object_base_link_name="base_link",)
# Generate plans.
plan_list = None
gripper_setpoint_list = None
if args.controller == "Trajectory":
plan_list, gripper_setpoint_list = GenerateOpenLeftDoorPlansByTrajectory()
elif args.controller == "Impedance" or args.controller == "Position":
plan_list, gripper_setpoint_list = GenerateOpenLeftDoorPlansByImpedanceOrPosition(
open_door_method=args.controller, is_open_fully=args.open_fully, handle_angle_start=theta0_hinge_new, handle_position=p_WC_handle_new, is_hardware=args.hardware)
print type(gripper_setpoint_list), gripper_setpoint_list
# Run simulator (simulation or hardware).
if is_hardware:
default=False,
help="Turns off visualization")
parser.add_argument(
"--profiling",
action="store_true",
default=False,
help="profile RunSimulation or RunRealRobot with cProfile")
parser.add_argument("--diagram_plan_runner",
action="store_true",
default=False,
help="Use the diagram version of plan_runner")
args = parser.parse_args()
is_hardware = args.hardware
# Construct simulator system.
manip_station_sim = ManipulationStationSimulator(time_step=2e-3)
# Generate plans.
plan_list, gripper_setpoint_list = GenerateExampleJointAndTaskSpacePlans()
# Run simulator (simulation or hardware).
if is_hardware:
if args.profiling:
cProfile.run(
"manip_station_sim.RunRealRobot(\
plan_list, gripper_setpoint_list,\
is_plan_runner_diagram=args.diagram_plan_runner)",
"stats_hardware")
else:
iiwa_position_command_log, iiwa_position_measured_log, iiwa_velocity_estimated_log, \
iiwa_external_torque_log, t_plan = \
def build(self, real_time_rate=0, is_visualizing=False):
# Create manipulation station simulator
self.manip_station_sim = ManipulationStationSimulator(
time_step=5e-3,
object_file_path=object_file_path,
object_base_link_name="base_link",
)
# Create plan runner
plan_runner, self.plan_scheduler = CreateManipStationPlanRunnerDiagram(
station=self.manip_station_sim.station,
kuka_plans=[],
gripper_setpoint_list=[],
rl_environment=True)
self.manip_station_sim.plan_runner = plan_runner
# Create builder and add systems
builder = DiagramBuilder()
builder.AddSystem(self.manip_station_sim.station)
builder.AddSystem(plan_runner)
# Connect systems
builder.Connect(
plan_runner.GetOutputPort("gripper_setpoint"),
self.manip_station_sim.station.GetInputPort("wsg_position"))
builder.Connect(
plan_runner.GetOutputPort("force_limit"),
self.manip_station_sim.station.GetInputPort("wsg_force_limit"))
demux = builder.AddSystem(Demultiplexer(14, 7))
builder.Connect(
plan_runner.GetOutputPort("iiwa_position_and_torque_command"),
demux.get_input_port(0))
builder.Connect(
demux.get_output_port(0),
self.manip_station_sim.station.GetInputPort("iiwa_position"))
builder.Connect(
demux.get_output_port(1),
self.manip_station_sim.station.GetInputPort(
"iiwa_feedforward_torque"))
builder.Connect(
self.manip_station_sim.station.GetOutputPort(
"iiwa_position_measured"),
plan_runner.GetInputPort("iiwa_position"))
builder.Connect(
self.manip_station_sim.station.GetOutputPort(
"iiwa_velocity_estimated"),
plan_runner.GetInputPort("iiwa_velocity"))
# Add meshcat visualizer
if is_visualizing:
scene_graph = self.manip_station_sim.station.get_mutable_scene_graph(
)
viz = MeshcatVisualizer(scene_graph,
is_drawing_contact_force=False,
plant=self.manip_station_sim.plant)
builder.AddSystem(viz)
builder.Connect(
self.manip_station_sim.station.GetOutputPort("pose_bundle"),
viz.GetInputPort("lcm_visualization"))
# Build diagram
self.diagram = builder.Build()
if is_visualizing:
print("Setting up visualizer...")
viz.load()
time.sleep(2.0)
# Construct Simulator
self.simulator = Simulator(self.diagram)
self.manip_station_sim.simulator = self.simulator
self.simulator.set_publish_every_time_step(False)
self.simulator.set_target_realtime_rate(real_time_rate)
self.context = self.diagram.GetMutableSubsystemContext(
self.manip_station_sim.station,
self.simulator.get_mutable_context())
self.left_hinge_joint = self.manip_station_sim.plant.GetJointByName(
"left_door_hinge")
self.right_hinge_joint = self.manip_station_sim.plant.GetJointByName(
"right_door_hinge")
# Goal for training
self.goal_position = np.array([0.85, 0, 0.31])
# Object body
self.obj = self.manip_station_sim.plant.GetBodyByName(
self.manip_station_sim.object_base_link_name,
self.manip_station_sim.object)
# Properties for RL
max_action = np.ones(8) * 0.1
max_action[-1] = 0.03
low_action = -1 * max_action
low_action[-1] = 0
self.action_space = ActionSpace(low=low_action, high=max_action)
self.state_dim = self._getObservation().shape[0]
self._episode_steps = 0
self._max_episode_steps = 75
# Set initial state of the robot
self.reset_sim = False
self.reset()
class ManipStationEnvironment(object):
def __init__(self, real_time_rate=0, is_visualizing=False):
# Store for continuity
self.is_visualizing = is_visualizing
self.real_time_rate = real_time_rate
self.build(real_time_rate=0, is_visualizing=is_visualizing)
def build(self, real_time_rate=0, is_visualizing=False):
# Create manipulation station simulator
self.manip_station_sim = ManipulationStationSimulator(
time_step=5e-3,
object_file_path=object_file_path,
object_base_link_name="base_link",
)
# Create plan runner
plan_runner, self.plan_scheduler = CreateManipStationPlanRunnerDiagram(
station=self.manip_station_sim.station,
kuka_plans=[],
gripper_setpoint_list=[],
rl_environment=True)
self.manip_station_sim.plan_runner = plan_runner
# Create builder and add systems
builder = DiagramBuilder()
builder.AddSystem(self.manip_station_sim.station)
builder.AddSystem(plan_runner)
# Connect systems
builder.Connect(
plan_runner.GetOutputPort("gripper_setpoint"),
self.manip_station_sim.station.GetInputPort("wsg_position"))
builder.Connect(
plan_runner.GetOutputPort("force_limit"),
self.manip_station_sim.station.GetInputPort("wsg_force_limit"))
demux = builder.AddSystem(Demultiplexer(14, 7))
builder.Connect(
plan_runner.GetOutputPort("iiwa_position_and_torque_command"),
demux.get_input_port(0))
builder.Connect(
demux.get_output_port(0),
self.manip_station_sim.station.GetInputPort("iiwa_position"))
builder.Connect(
demux.get_output_port(1),
self.manip_station_sim.station.GetInputPort(
"iiwa_feedforward_torque"))
builder.Connect(
self.manip_station_sim.station.GetOutputPort(
"iiwa_position_measured"),
plan_runner.GetInputPort("iiwa_position"))
builder.Connect(
self.manip_station_sim.station.GetOutputPort(
"iiwa_velocity_estimated"),
plan_runner.GetInputPort("iiwa_velocity"))
# Add meshcat visualizer
if is_visualizing:
scene_graph = self.manip_station_sim.station.get_mutable_scene_graph(
)
viz = MeshcatVisualizer(scene_graph,
is_drawing_contact_force=False,
plant=self.manip_station_sim.plant)
builder.AddSystem(viz)
builder.Connect(
self.manip_station_sim.station.GetOutputPort("pose_bundle"),
viz.GetInputPort("lcm_visualization"))
# Build diagram
self.diagram = builder.Build()
if is_visualizing:
print("Setting up visualizer...")
viz.load()
time.sleep(2.0)
# Construct Simulator
self.simulator = Simulator(self.diagram)
self.manip_station_sim.simulator = self.simulator
self.simulator.set_publish_every_time_step(False)
self.simulator.set_target_realtime_rate(real_time_rate)
self.context = self.diagram.GetMutableSubsystemContext(
self.manip_station_sim.station,
self.simulator.get_mutable_context())
self.left_hinge_joint = self.manip_station_sim.plant.GetJointByName(
"left_door_hinge")
self.right_hinge_joint = self.manip_station_sim.plant.GetJointByName(
"right_door_hinge")
# Goal for training
self.goal_position = np.array([0.85, 0, 0.31])
# Object body
self.obj = self.manip_station_sim.plant.GetBodyByName(
self.manip_station_sim.object_base_link_name,
self.manip_station_sim.object)
# Properties for RL
max_action = np.ones(8) * 0.1
max_action[-1] = 0.03
low_action = -1 * max_action
low_action[-1] = 0
self.action_space = ActionSpace(low=low_action, high=max_action)
self.state_dim = self._getObservation().shape[0]
self._episode_steps = 0
self._max_episode_steps = 75
# Set initial state of the robot
self.reset_sim = False
self.reset()
def step(self, action):
assert len(action) == 8
next_plan = JointSpacePlanRelative(delta_q=action[:-1], duration=0.1)
sim_duration = self.plan_scheduler.setNextPlan(next_plan, 0.05)
try:
self.simulator.StepTo(sim_duration)
except:
self.reset_sim = True
return self._getObservation(), -999, True, None
reward = self._getReward()
if reward > -0.1:
done = True
else:
self._episode_steps += 1
if self._episode_steps == self._max_episode_steps:
done = True
else:
done = False
return self._getObservation(), reward, done, None
def reset(self):
if self.reset_sim:
print("Resetting")
self.build(real_time_rate=self.real_time_rate,
is_visualizing=self.is_visualizing)
while True:
p_WQ_new = np.random.uniform(low=[0.05, -0.1, 0.5],
high=[0.5, 0.1, 0.5])
# p_WQ_new = np.array([0.2, 0, 0.5])
passed, q_home_full = GetConfiguration(p_WQ_new)
if passed:
break
q_home_kuka = GetKukaQKnots(q_home_full)[0]
# set initial hinge angles of the cupboard.
# setting hinge angle to exactly 0 or 90 degrees will result in intermittent contact
# with small contact forces between the door and the cupboard body.
self.left_hinge_joint.set_angle(
context=self.manip_station_sim.station.GetMutableSubsystemContext(
self.manip_station_sim.plant, self.context),
angle=-np.pi / 2 + 0.001)
self.right_hinge_joint.set_angle(
context=self.manip_station_sim.station.GetMutableSubsystemContext(
self.manip_station_sim.plant, self.context),
angle=np.pi / 2 - 0.001)
# set initial pose of the object
self.manip_station_sim.SetObjectTranslation(self.goal_position)
if self.manip_station_sim.object_base_link_name is not None:
self.manip_station_sim.tree.SetFreeBodyPoseOrThrow(
self.manip_station_sim.plant.GetBodyByName(
self.manip_station_sim.object_base_link_name,
self.manip_station_sim.object),
self.manip_station_sim.X_WObject,
self.manip_station_sim.station.GetMutableSubsystemContext(
self.manip_station_sim.plant, self.context))
if self.manip_station_sim.object_base_link_name is not None:
self.manip_station_sim.tree.SetFreeBodySpatialVelocityOrThrow(
self.manip_station_sim.plant.GetBodyByName(
self.manip_station_sim.object_base_link_name,
self.manip_station_sim.object),
SpatialVelocity(np.zeros(3), np.zeros(3)),
self.manip_station_sim.station.GetMutableSubsystemContext(
self.manip_station_sim.plant, self.context))
# set initial state of the robot
self.manip_station_sim.station.SetIiwaPosition(q_home_kuka,
self.context)
self.manip_station_sim.station.SetIiwaVelocity(np.zeros(7),
self.context)
self.manip_station_sim.station.SetWsgPosition(0.02, self.context)
self.manip_station_sim.station.SetWsgVelocity(0, self.context)
self.simulator.Initialize()
self._episode_steps = 0
return self._getObservation()
def seed(self, seed):
np.random.seed(seed)
def _getObservation(self):
kuka_position = self.manip_station_sim.station.GetIiwaPosition(
self.context)
object_position = self.manip_station_sim.tree.EvalBodyPoseInWorld(
context=self.manip_station_sim.station.GetMutableSubsystemContext(
self.manip_station_sim.plant, self.context),
body=self.obj).translation()
return np.append(kuka_position, object_position)
def _getReward(self):
# object_position = self.manip_station_sim.tree.EvalBodyPoseInWorld(
# context=self.manip_station_sim.station.GetMutableSubsystemContext(self.manip_station_sim.plant, self.context),
# body=self.obj).translation()
gripper_pose = self.manip_station_sim.tree.CalcRelativeTransform(
context=self.manip_station_sim.station.GetMutableSubsystemContext(
self.manip_station_sim.plant, self.context),
frame_A=self.manip_station_sim.world_frame,
frame_B=self.manip_station_sim.gripper_frame).translation()
dist = np.linalg.norm(self.goal_position - gripper_pose)
return -dist
def main():
time_step = 2e-3
parser = argparse.ArgumentParser()
parser.add_argument('-c',
'--cfree',
action='store_true',
help='Disables collisions when planning')
parser.add_argument(
'-d',
'--deterministic',
action='store_true',
help='Manually sets the random seeds used by the stream generators')
parser.add_argument('-s',
'--simulate',
action='store_true',
help='Simulates the system')
parser.add_argument('-e',
'--execute',
action='store_true',
help='Executes the system')
parser.add_argument('-l',
'--load',
action='store_true',
help='Loads the last plan')
parser.add_argument('-f',
'--force_control',
action='store_true',
help='Use impedance control to open the door')
parser.add_argument('-p',
'--poses',
default=DOPE_PATH,
help='The path to the dope poses file')
args = parser.parse_args()
if args.deterministic:
random.seed(0)
np.random.seed(0)
goal_name = 'soup'
if args.poses == 'none':
task, diagram, state_machine = load_station(time_step=time_step)
else:
task, diagram, state_machine = load_dope(time_step=time_step,
dope_path=args.poses,
goal_name=goal_name)
print(task)
plant = task.mbp
RenderSystemWithGraphviz(diagram) # Useful for getting port names
task.publish()
context = diagram.GetMutableSubsystemContext(plant, task.diagram_context)
world_frame = plant.world_frame()
tree = plant.tree()
X_WSoup = tree.CalcRelativeTransform(
context,
frame_A=world_frame,
frame_B=plant.GetFrameByName("base_link_soup"))
if not args.load:
replan(task,
context,
visualize=True,
collisions=not args.cfree,
use_impedance=args.force_control)
##################################################
if not args.simulate and not args.execute:
return
manip_station_sim = ManipulationStationSimulator(
time_step=time_step,
object_file_path=get_sdf_path(goal_name),
object_base_link_name="base_link_soup",
X_WObject=X_WSoup)
plan_list = []
gripper_setpoints = []
splines = np.load("splines.npy")
setpoints = np.load("gripper_setpoints.npy")
for control, setpoint in zip(splines, setpoints):
if isinstance(control, ForceControl):
new_plans, new_setpoints = \
GenerateOpenLeftDoorPlansByImpedanceOrPosition("Impedance", is_open_fully=True)
plan_list.extend(new_plans)
gripper_setpoints.extend(new_setpoints)
else:
plan_list.append(control.plan())
gripper_setpoints.append(setpoint)
dump_plans(plan_list, gripper_setpoints)
sim_duration = compute_duration(plan_list)
print('Splines: {}\nDuration: {:.3f} seconds'.format(
len(plan_list), sim_duration))
if args.execute:
raw_input('Execute on hardware?')
iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log = \
manip_station_sim.RunRealRobot(plan_list, gripper_setpoints)
#PlotExternalTorqueLog(iiwa_external_torque_log)
#PlotIiwaPositionLog(iiwa_position_command_log, iiwa_position_measured_log)
else:
raw_input('Execute in simulation?')
q0 = [0, 0, 0, -1.75, 0, 1.0, 0]
iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, \
plant_state_log = \
manip_station_sim.RunSimulation(plan_list, gripper_setpoints,
extra_time=2.0, real_time_rate=1.0, q0_kuka=q0)
)
args = parser.parse_args()
object_file_path = FindResourceOrThrow(
'drake/examples/manipulation_station/models/061_foam_brick.sdf'
)
obstacle_file_path = "motion_planner/models/cracker_box.sdf"
X_WObject = Isometry3.Identity()
X_WObject.set_translation([.6, 0, 0])
X_WObstacle = Isometry3.Identity()
X_WObstacle.set_translation([0.4, 0, 0])
manip_station_sim = ManipulationStationSimulator(
time_step=2e-3,
object_file_paths=[object_file_path, obstacle_file_path],
object_base_link_names=['base_link', 'base_link_cracker'],
X_WObject_list=[X_WObject, X_WObstacle]
)
# initial q and goal p
q0 = [0, 0, 0, -1.75, 0, 1.0, 0]
p_WC_box = np.array([0.6, 0.05 / 2, 0.025 + 0.025])
algo = args.algorithm
max_iter = args.max_iter
num_runs = args.num_runs
motion_planning = MotionPlanning(q_initial=q0,
p_goal=p_WC_box,
object_file_paths=[object_file_path, obstacle_file_path],
object_base_link_names=['base_link', 'base_link_cracker'],
default=300,
help="Specify the maximum iterations the algorithm is allowed to run")
args = parser.parse_args()
object_file_path = FindResourceOrThrow(
'drake/examples/manipulation_station/models/061_foam_brick.sdf')
obstacle_file_path = "motion_planner/models/cracker_box.sdf"
X_WObject = Isometry3.Identity()
X_WObject.set_translation([.6, 0, 0])
X_WObstacle = Isometry3.Identity()
X_WObstacle.set_translation([0.4, 0, 0])
manip_station_sim = ManipulationStationSimulator(
time_step=2e-3,
object_file_paths=[object_file_path, obstacle_file_path],
object_base_link_names=['base_link', 'base_link_cracker'],
X_WObject_list=[X_WObject, X_WObstacle])
# initial q and goal p
q0 = [0, 0, 0, -1.75, 0, 1.0, 0]
p_WC_box = np.array([0.6, 0.05 / 2, 0.025 + 0.025])
algo = args.algorithm
max_iter = args.max_iter
motion_planning = MotionPlanning(
q_initial=q0,
p_goal=p_WC_box,
object_file_paths=[object_file_path, obstacle_file_path],
object_base_link_names=['base_link', 'base_link_cracker'],
class TestPDDLPlanning(unittest.TestCase):
def setUp(self):
self.q0 = [0, 0, 0, -1.75, 0, 1.0, 0]
self.time_step = 2e-3
self.prevdir = os.getcwd()
os.chdir(os.path.expanduser("pddl_planning"))
task, diagram, state_machine = load_dope(time_step=self.time_step,
dope_path="poses.txt",
goal_name="soup",
is_visualizing=False)
plant = task.mbp
task.publish()
context = diagram.GetMutableSubsystemContext(plant, task.diagram_context)
world_frame = plant.world_frame()
tree = plant.tree()
X_WSoup = tree.CalcRelativeTransform(
context, frame_A=world_frame, frame_B=plant.GetFrameByName("base_link_soup"))
self.manip_station_sim = ManipulationStationSimulator(
time_step=self.time_step,
object_file_path="./models/ycb_objects/soup_can.sdf",
object_base_link_name="base_link_soup",
X_WObject=X_WSoup)
def tearDown(self):
os.chdir(self.prevdir)
def InspectLog(self, state_log, plant):
tree = plant.tree()
data = state_log.data()
# create a context of final state.
x_final = data[:, -1]
context = plant.CreateDefaultContext()
x_mutable = tree.GetMutablePositionsAndVelocities(context)
x_mutable[:] = x_final
# cupboard must be open.
hinge_joint = plant.GetJointByName("left_door_hinge")
joint_angle = hinge_joint.get_angle(context)
self.assertTrue(np.abs(joint_angle) > np.pi/6,
"Cupboard door is not fully open.")
# velocity must be small throughout the simulation.
for x in data.T:
v = x[plant.num_positions():]
self.assertTrue((np.abs(v) < 3.).all(), "velocity is too large.")
def HasReturnedToQtarget(self, q_iiwa_target, state_log, plant):
tree = plant.tree()
data = state_log.data()
q_final = data[:, -1][:plant.num_positions()]
iiwa_model = plant.GetModelInstanceByName("iiwa")
q_iiwa_final = tree.GetPositionsFromArray(iiwa_model, q_final)
return (np.abs(q_iiwa_target - q_iiwa_final) < 0.03).all()
def test_pddl(self):
splines = np.load("test_data/splines.npy")
setpoints = np.load("test_data/gripper_setpoints.npy")
plan_list = []
gripper_setpoints = []
for control, setpoint in zip(splines, setpoints):
plan_list.append(control.plan())
gripper_setpoints.append(setpoint)
sim_duration = compute_duration(plan_list)
q_iiwa_beginning = plan_list[0].traj.value(0).flatten()
iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, \
plant_state_log, t_plan = \
self.manip_station_sim.RunSimulation(plan_list, gripper_setpoints,
extra_time=2.0, real_time_rate=0.0,
q0_kuka=self.q0, is_visualizing=False)
# Run Tests
self.InspectLog(plant_state_log, self.manip_station_sim.plant)
self.assertTrue(
self.HasReturnedToQtarget(q_iiwa_beginning, plant_state_log, self.manip_station_sim.plant))
def test_pddl_force_control(self):
splines = np.load("test_data/splines_force_control.npy")
setpoints = np.load("test_data/gripper_setpoints_force_control.npy")
plan_list = []
gripper_setpoints = []
for control, setpoint in zip(splines, setpoints):
if isinstance(control, ForceControl):
new_plans, new_setpoints = \
GenerateOpenLeftDoorPlansByImpedanceOrPosition("Impedance", is_open_fully=True)
plan_list.extend(new_plans)
gripper_setpoints.extend(new_setpoints)
else:
plan_list.append(control.plan())
gripper_setpoints.append(setpoint)
sim_duration = compute_duration(plan_list)
q_iiwa_beginning = plan_list[0].traj.value(0).flatten()
iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, \
plant_state_log, t_plan = \
self.manip_station_sim.RunSimulation(plan_list, gripper_setpoints,
extra_time=2.0, real_time_rate=0.0,
q0_kuka=self.q0, is_visualizing=False)
# Run Tests
self.InspectLog(plant_state_log, self.manip_station_sim.plant)
self.assertTrue(
self.HasReturnedToQtarget(q_iiwa_beginning, plant_state_log, self.manip_station_sim.plant))