def playbackMotion(data1, data2, data3, data4, times):
data = np.concatenate((data2, data1, data4, data3), axis=0)
tree = RigidBodyTree(
FindResource(
os.path.dirname(os.path.realpath(__file__)) +
"/block_pusher2.urdf"), FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics), the controller, and a
# visualization block.
builder = DiagramBuilder()
robot = builder.AddSystem(Player(data, times))
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
Tview,
xlim=[-2.8, 4.8],
ylim=[-2.8, 10]))
#print(robot.get_output_port(0).size())
builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0))
logger = builder.AddSystem(
SignalLogger(tree.get_num_positions() + tree.get_num_velocities()))
builder.Connect(robot.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(True)
# Simulate for 10 seconds
simulator.StepTo(times[-1] + 0.5)
def simulateRobot(time, B, v_command):
tree = RigidBodyTree(
FindResource(
os.path.dirname(os.path.realpath(__file__)) +
"/block_pusher2.urdf"), FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics), the controller, and a
# visualization block.
builder = DiagramBuilder()
robot = builder.AddSystem(RigidBodyPlant(tree))
controller = builder.AddSystem(DController(tree, B, v_command))
builder.Connect(robot.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), robot.get_input_port(0))
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
Tview,
xlim=[-2.8, 4.8],
ylim=[-2.8, 10]))
builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0))
logger = builder.AddSystem(
SignalLogger(tree.get_num_positions() + tree.get_num_velocities()))
builder.Connect(robot.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(True)
# Set the initial conditions
context = simulator.get_mutable_context()
state = context.get_mutable_continuous_state_vector()
start1 = 3 * np.pi / 16
start2 = 15 * np.pi / 16
#np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps, np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps
state.SetFromVector(
(start1, start2, -start1, -start2, np.pi + start1, start2,
np.pi - start1, -start2, 1, 1, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
0., 0.)) # (theta1, theta2, theta1dot, theta2dot)
# Simulate for 10 seconds
simulator.StepTo(time)
#import pdb; pdb.set_trace()
return (logger.data()[8:11, :], logger.data()[:8, :],
logger.data()[19:22, :], logger.data()[11:19, :],
logger.sample_times())
def do_cut(self, rbt, x, cut_body_index, cut_pt, cut_normal):
# Rebuilds the full rigid body tree, replacing cut_body_index
# with one that is cut, but otherwise keeping the rest of the
# tree the same. The new tree won't have the same body indices
# (for the manipulands and anything added after them) as the
# original.
old_manipuland_indices = deepcopy(self.manipuland_body_indices)
old_manipuland_params = deepcopy(self.manipuland_params)
self.__init__()
new_rbt = RigidBodyTree()
self.setup_kuka(new_rbt)
q_new = np.zeros(rbt.get_num_positions() + 6)
v_new = np.zeros(rbt.get_num_positions() + 6)
if rbt.get_num_positions() != rbt.get_num_velocities():
raise Exception("Can't handle nq != nv, sorry...")
def copy_state(from_indices, to_indices):
q_new[to_indices] = x[from_indices]
v_new[to_indices] = x[np.array(from_indices) +
rbt.get_num_positions()]
copy_state(range(new_rbt.get_num_positions()),
range(new_rbt.get_num_positions()))
k = 0
for i, ind in enumerate(old_manipuland_indices):
print i, ind
p = old_manipuland_params[i]
if ind is cut_body_index:
for sign in [-1., 1.]:
try:
self.add_cut_cylinder_to_tabletop(
new_rbt,
height=p["height"],
radius=p["radius"],
cut_dirs=p["cut_dirs"] + [cut_normal * sign],
cut_points=p["cut_points"] +
[cut_pt + cut_normal * sign * 0.002])
except subprocess.CalledProcessError as e:
print "Failed a cut: ", e
continue # failed to cut
k += 1
copy_state(
range(
rbt.get_body(ind).get_position_start_index(),
rbt.get_body(ind).get_position_start_index() + 6),
range(new_rbt.get_num_positions() - 6,
new_rbt.get_num_positions()))
else:
self.add_cut_cylinder_to_tabletop(new_rbt,
height=p["height"],
radius=p["radius"],
cut_dirs=p["cut_dirs"],
cut_points=p["cut_points"])
copy_state(
range(
rbt.get_body(ind).get_position_start_index(),
rbt.get_body(ind).get_position_start_index() + 6),
range(new_rbt.get_num_positions() - 6,
new_rbt.get_num_positions()))
k += 1
# Account for possible cut failures
q_new = q_new[:new_rbt.get_num_positions()]
v_new = v_new[:new_rbt.get_num_velocities()]
# Map old state into new state
new_rbt.compile()
q_new = self.project_rbt_to_nearest_feasible_on_table(new_rbt, q_new)
return new_rbt, np.hstack([q_new, v_new])
def getPredictedMotion(B, v_command, time):
#object_positions = object_positions + 0.1
#manipulator_positions = manipulator_positions + 0.1
#object_velocities = object_velocities + 0.1
#manipulator_velocities = manipulator_velocities + 0.1
#object_positions = object_positions[:, range(0,object_positions.shape[1],2)]
#manipulator_positions = manipulator_positions[:, range(0,manipulator_positions.shape[1],2)]
#object_velocities = object_velocities[:, range(0,object_velocities.shape[1],2)]
#manipulator_velocities = manipulator_velocities[:, range(0,manipulator_velocities.shape[1],2)]
#times = times[range(0,times.size,2)]
#import pdb; pdb.set_trace()
step = 0.01
A = 10 * np.eye(3)
tree = RigidBodyTree(
FindResource(
os.path.dirname(os.path.realpath(__file__)) +
"/block_pusher2.urdf"), FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics), the controller, and a
# visualization block.
builder = DiagramBuilder()
#robot = builder.AddSystem(RigidBodyPlant(tree))
robot = builder.AddSystem(
QuasiStaticRigidBodyPlant(tree, step, A, np.linalg.inv(B)))
controller = builder.AddSystem(QController(tree, v_command, step))
#builder.Connect(robot.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), robot.get_input_port(0))
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
Tview,
xlim=[-2.8, 4.8],
ylim=[-2.8, 10]))
builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0))
logger = builder.AddSystem(
SignalLogger(tree.get_num_positions() + tree.get_num_velocities()))
builder.Connect(robot.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(True)
# Set the initial conditions
context = simulator.get_mutable_context()
state = context.get_mutable_discrete_state_vector()
start1 = 3 * np.pi / 16
start2 = 15 * np.pi / 16
#np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps, np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps
state.SetFromVector(
(start1, start2, -start1, -start2, np.pi + start1, start2,
np.pi - start1, -start2, 1, 1, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
0., 0.)) # (theta1, theta2, theta1dot, theta2dot)
# Simulate for 10 seconds
simulator.StepTo(time)
#import pdb; pdb.set_trace()
return (logger.data()[8:11, :], logger.data()[:8, :],
logger.data()[19:22, :], logger.data()[11:19, :],
logger.sample_times())
builder = DiagramBuilder()
# build the experiment system
tree = RigidBodyTree()
AddFlatTerrainToWorld(tree, 100, 10)
addKukaSetup(tree,collision_type='spheres')
# create the diagram
plant = RigidBodyPlant(tree, 0.0001) # 2000 Hz
deltas = [-0.66667,-0.33333,0.0,0.5,1.0]
for delta in deltas:
# print "delta: " + str(delta)
for i in range(tree.get_num_positions()+tree.get_num_velocities()):
# print "index: " + str(i)
xk = np.zeros(tree.get_num_positions()+tree.get_num_velocities())
# xk = np.array([0.3689,0.4607,0.9816,0.1564,0.8555,0.6448,0.3763,0.1909,0.4283,0.4820,0.1206,0.5895,0.2262,0.3846])
xk[i] += delta
str_out = ''
for j in range(tree.get_num_positions()+tree.get_num_velocities()):
str_out += "%.6f " % xk[j]
print str_out + "\n"
kinsol = tree.doKinematics(xk[:tree.get_num_positions()],xk[tree.get_num_positions():])
massMatrix = tree.massMatrix(kinsol)
biasTerm = tree.dynamicsBiasTerm(kinsol,{})
str_out = ''
for j in range(tree.get_num_positions()):
for k in range(tree.get_num_positions()):
str_out += "%.4f " % massMatrix[j][k]
# tree = RigidBodyTree(FindResource("cubli/cubli.sdf"),
# FloatingBaseType.kFixed)
builder = DiagramBuilder()
# AddModelInstancesFromSdfString(
# open("underactuated/src/cubli/cubli.sdf", 'r').read(),
# FloatingBaseType.kFixed,
# None, tree)
# tree
tree = RigidBodyTree(FindResource("cubli/cubli.urdf"), FloatingBaseType.kFixed)
#
# AddFlatTerrainToWorld(tree, 1000, -1)
# plant = RigidBodyPlant(tree, timestep)
plant = RigidBodyPlant(tree)
nx = tree.get_num_positions() + tree.get_num_velocities()
allmaterials = CompliantMaterial()
allmaterials.set_youngs_modulus(1E9) # default 1E9
allmaterials.set_dissipation(1.0) # default 0.32
allmaterials.set_friction(1.0) # default 0.9.
plant.set_default_compliant_material(allmaterials)
context = plant.CreateDefaultContext()
print(tree.get_num_positions())
# ETHAN
robot = builder.AddSystem(plant)
# builder.ExportInput(robot.get_input_port(0))