def setupValkyrieExample():
# Valkyrie Example
rbt = RigidBodyTree()
world_frame = RigidBodyFrame("world_frame", rbt.world(), [0, 0, 0],
[0, 0, 0])
from pydrake.multibody.parsers import PackageMap
pmap = PackageMap()
# Note: Val model is currently not installed in drake binary distribution.
pmap.PopulateFromFolder(os.path.join(pydrake.getDrakePath(), "examples"))
# TODO(russt): remove plane.urdf and call AddFlatTerrainTOWorld instead
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(FindResource(os.path.join("underactuated", "plane.urdf")),
'r').read(), # noqa
pmap,
pydrake.getDrakePath() + "/examples/",
FloatingBaseType.kFixed,
world_frame,
rbt)
val_start_frame = RigidBodyFrame("val_start_frame", rbt.world(),
[0, 0, 1.5], [0, 0, 0])
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(
pydrake.getDrakePath() +
"/examples/valkyrie/urdf/urdf/valkyrie_A_sim_drake_one_neck_dof_wide_ankle_rom.urdf",
'r').read(), # noqa
pmap,
pydrake.getDrakePath() + "/examples/",
FloatingBaseType.kRollPitchYaw,
val_start_frame,
rbt)
Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
pbrv = MeshcatRigidBodyVisualizer(rbt)
return rbt, pbrv
def BuildHand(num_fingers=3, manipuland_sdf="models/manipuland_box.sdf"):
''' Build up the hand by replicating a finger
model at a handful of base positions. '''
finger_base_positions = np.vstack([
np.abs(np.linspace(-0.25, 0.25, num_fingers)),
np.linspace(0.25, -0.25, num_fingers),
np.zeros(num_fingers)
]).T
tree = RigidBodyTree()
for i, base_pos in enumerate(finger_base_positions):
frame = RigidBodyFrame(name="finger_%d_base_frame" % i,
body=tree.world(),
xyz=base_pos,
rpy=[0, 0, 0])
tree.addFrame(frame)
AddModelInstancesFromSdfString(
open("models/planar_finger.sdf", 'r').read(),
FloatingBaseType.kFixed, frame, tree)
# Add the manipuland as well from an sdf.
manipuland_frame = RigidBodyFrame(name="manipuland_frame",
body=tree.world(),
xyz=[0, 0., 0.],
rpy=[0, 0, 0])
tree.addFrame(manipuland_frame)
AddModelInstancesFromSdfString(
open(manipuland_sdf, 'r').read(), FloatingBaseType.kFixed,
manipuland_frame, tree)
return tree
def setup_kuka(self, rbt):
iiwa_urdf_path = os.path.join(
pydrake.getDrakePath(),
"manipulation", "models", "iiwa_description", "urdf",
"iiwa14_polytope_collision.urdf")
wsg50_sdf_path = os.path.join(
pydrake.getDrakePath(),
"manipulation", "models", "wsg_50_description", "sdf",
"schunk_wsg_50.sdf")
table_sdf_path = os.path.join(
pydrake.getDrakePath(),
"examples", "kuka_iiwa_arm", "models", "table",
"extra_heavy_duty_table_surface_only_collision.sdf")
guillotine_path = "guillotine.sdf"
AddFlatTerrainToWorld(rbt)
table_frame_robot = RigidBodyFrame(
"table_frame_robot", rbt.world(),
[0.0, 0, 0], [0, 0, 0])
self.add_model_wrapper(table_sdf_path, FloatingBaseType.kFixed,
table_frame_robot, rbt)
self.tabletop_indices.append(rbt.get_num_bodies()-1)
table_frame_fwd = RigidBodyFrame(
"table_frame_fwd", rbt.world(),
[0.7, 0, 0], [0, 0, 0])
self.add_model_wrapper(table_sdf_path, FloatingBaseType.kFixed,
table_frame_fwd, rbt)
self.tabletop_indices.append(rbt.get_num_bodies()-1)
robot_base_frame = RigidBodyFrame(
"robot_base_frame", rbt.world(),
[0.0, 0, self.table_top_z_in_world], [0, 0, 0])
self.add_model_wrapper(iiwa_urdf_path, FloatingBaseType.kFixed,
robot_base_frame, rbt)
# Add gripper
gripper_frame = rbt.findFrame("iiwa_frame_ee")
self.add_model_wrapper(wsg50_sdf_path, FloatingBaseType.kFixed,
gripper_frame, rbt)
if self.with_knife:
# Add guillotine
guillotine_frame = RigidBodyFrame(
"guillotine_frame", rbt.world(),
[0.6, -0.41, self.table_top_z_in_world], [0, 0, 0])
self.add_model_wrapper(guillotine_path, FloatingBaseType.kFixed,
guillotine_frame, rbt)
self.guillotine_blade_index = \
rbt.FindBody("blade").get_body_index()
def setup_kuka(rbt):
iiwa_urdf_path = os.path.join(pydrake.getDrakePath(), "manipulation",
"models", "iiwa_description", "urdf",
"iiwa14_polytope_collision.urdf")
wsg50_sdf_path = os.path.join(pydrake.getDrakePath(), "manipulation",
"models", "wsg_50_description", "sdf",
"schunk_wsg_50.sdf")
table_sdf_path = os.path.join(
pydrake.getDrakePath(), "examples", "kuka_iiwa_arm", "models", "table",
"extra_heavy_duty_table_surface_only_collision.sdf")
object_urdf_path = os.path.join(pydrake.getDrakePath(), "examples",
"kuka_iiwa_arm", "models", "objects",
"block_for_pick_and_place.urdf")
AddFlatTerrainToWorld(rbt)
table_frame_robot = RigidBodyFrame("table_frame_robot", rbt.world(),
[0.0, 0, 0], [0, 0, 0])
AddModelInstancesFromSdfString(
open(table_sdf_path).read(), FloatingBaseType.kFixed,
table_frame_robot, rbt)
table_frame_fwd = RigidBodyFrame("table_frame_fwd", rbt.world(),
[0.8, 0, 0], [0, 0, 0])
AddModelInstancesFromSdfString(
open(table_sdf_path).read(), FloatingBaseType.kFixed, table_frame_fwd,
rbt)
table_top_z_in_world = 0.736 + 0.057 / 2
robot_base_frame = RigidBodyFrame("robot_base_frame", rbt.world(),
[0.0, 0, table_top_z_in_world],
[0, 0, 0])
AddModelInstanceFromUrdfFile(iiwa_urdf_path, FloatingBaseType.kFixed,
robot_base_frame, rbt)
object_init_frame = RigidBodyFrame("object_init_frame", rbt.world(),
[0.8, 0, table_top_z_in_world + 0.1],
[0, 0, 0])
AddModelInstanceFromUrdfFile(object_urdf_path,
FloatingBaseType.kRollPitchYaw,
object_init_frame, rbt)
# Add gripper
gripper_frame = rbt.findFrame("iiwa_frame_ee")
AddModelInstancesFromSdfString(
open(wsg50_sdf_path).read(), FloatingBaseType.kFixed, gripper_frame,
rbt)
def do_model_pointcloud_sampling(args, config, vis=None, vis_prefix=None):
# For each class, sample model points on its surface.
for class_i, class_name in enumerate(config["objects"].keys()):
class_rbt = RigidBodyTree()
frame = RigidBodyFrame("frame", class_rbt.world(), np.zeros(3),
np.zeros(3))
model_path = config["objects"][class_name]["model_path"]
_, extension = os.path.splitext(model_path)
if extension == ".urdf":
AddModelInstanceFromUrdfFile(model_path, FloatingBaseType.kFixed,
frame, class_rbt)
elif extension == ".sdf":
AddModelInstancesFromSdfString(
open(model_path).read(), FloatingBaseType.kFixed, frame,
class_rbt)
else:
raise ValueError("Class %s has non-sdf and non-urdf model name." %
class_name)
# Sample model points
model_points, model_normals = sample_points_on_body(
class_rbt, 1, 0.005)
print "Sampled %d model points from model %s" % (model_points.shape[1],
class_name)
save_pointcloud(model_points, model_normals,
os.path.join(args.dir, "%s.pc" % (class_name)))
if vis:
model_pts_offset = (model_points.T + [class_i, 0., -1.0]).T
draw_points(vis,
vis_prefix,
class_name,
model_pts_offset,
model_normals,
size=0.0005,
normals_length=0.00)
def setup_scene(rbt, config):
if config["with_ground"] is True:
AddFlatTerrainToWorld(rbt)
for i, instance_config in enumerate(config["instances"]):
add_single_instance_to_rbt(rbt,
config,
instance_config,
i,
floating_base_type=FloatingBaseType.kFixed)
# Add camera geometry!
camera_link = RigidBody()
camera_link.set_name("camera_link")
# necessary so this last link isn't pruned by the rbt.compile() call
camera_link.set_spatial_inertia(np.eye(6))
camera_link.add_joint(
rbt.world(),
RollPitchYawFloatingJoint("camera_floating_base", np.eye(4)))
rbt.add_rigid_body(camera_link)
# - Add frame for camera fixture.
camera_frame = RigidBodyFrame(name="rgbd_camera_frame",
body=camera_link,
xyz=[0.0, 0., 0.],
rpy=[0., 0., 0.])
rbt.addFrame(camera_frame)
rbt.compile()
def add_single_instance_to_rbt(
rbt,
config,
instance_config,
i,
floating_base_type=FloatingBaseType.kRollPitchYaw):
class_name = instance_config["class"]
if class_name not in config["objects"].keys():
raise ValueError("Class %s not in classes." % class_name)
if len(instance_config["pose"]) != 6:
raise ValueError("Class %s has pose size != 6. Use RPY plz" %
class_name)
frame = RigidBodyFrame("%s_%d" % (class_name, i), rbt.world(),
instance_config["pose"][0:3],
instance_config["pose"][3:6])
model_path = config["objects"][class_name]["model_path"]
_, extension = os.path.splitext(model_path)
if extension == ".urdf":
AddModelInstanceFromUrdfFile(model_path, floating_base_type, frame,
rbt)
elif extension == ".sdf":
AddModelInstancesFromSdfString(
open(model_path).read(), floating_base_type, frame, rbt)
else:
raise ValueError("Class %s has non-sdf and non-urdf model name." %
class_name)
def addKukaSetup(tree, collision_type="none"):
world_frame = RigidBodyFrame("world_frame", tree.world(), [0, 0, 0],[0, 0, 0])
# AddModelInstanceFromUrdfFile("models/kuka_table.urdf",FloatingBaseType.kFixed, world_frame, tree)
kuka_urdf_path = getKukaUrdfPath(collision_type=collision_type)
# table_top_frame = tree.findFrame("kuka_table_top")
# AddModelInstanceFromUrdfFile(kuka_urdf_path, FloatingBaseType.kFixed,table_top_frame, tree)
AddModelInstanceFromUrdfFile(kuka_urdf_path, FloatingBaseType.kFixed,world_frame, tree)
def add_cut_cylinder_to_tabletop(self,
rbt,
model_name=None,
do_convex_decomp=False,
height=None,
radius=None,
cut_dirs=None,
cut_points=None):
if model_name is None:
timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%m%S%f")
model_name = "cyl_" + timestamp
import mesh_creation
import trimesh
# Determine parameters of the cylinders
height = height or np.random.random() * 0.05 + 0.1
radius = radius or np.random.random() * 0.02 + 0.01
if cut_dirs is None:
cut_dirs = [np.array([1., 0., 0.])]
if cut_points is None:
cut_points = [
np.array([(np.random.random() - 0.5) * radius * 1., 0, 0])
]
cutting_planes = zip(cut_points, cut_dirs)
print "Cutting with cutting planes ", cutting_planes
# Create a mesh programmatically for that cylinder
cyl = mesh_creation.create_cut_cylinder(radius,
height,
cutting_planes,
sections=6)
cyl.density = 1000. # Same as water
self.manipuland_params.append(
dict(height=height,
radius=radius,
cut_dirs=cut_dirs,
cut_points=cut_points))
# Save it out to a file and add it to the RBT
object_init_frame = RigidBodyFrame("object_init_frame_%s" % model_name,
rbt.world(), np.zeros(3),
self.magic_rpy_offset)
if do_convex_decomp: # more powerful, does a convex decomp
urdf_dir = "/tmp/%s/" % model_name
trimesh.io.urdf.export_urdf(cyl, urdf_dir)
urdf_path = urdf_dir + "%s.urdf" % model_name
self.add_model_wrapper(urdf_path, FloatingBaseType.kRollPitchYaw,
object_init_frame, rbt)
self.manipuland_body_indices.append(rbt.get_num_bodies() - 1)
else:
sdf_dir = "/tmp/%s/" % model_name
file_name = "%s" % model_name
mesh_creation.export_sdf(cyl,
file_name,
sdf_dir,
color=[0.75, 0.5, 0.2, 1.])
sdf_path = sdf_dir + "%s.sdf" % model_name
self.add_model_wrapper(sdf_path, FloatingBaseType.kRollPitchYaw,
object_init_frame, rbt)
self.manipuland_body_indices.append(rbt.get_num_bodies() - 1)
def add_robot(rbt): # type: (RigidBodyTree) -> None
# The iiwa link path
iiwa_urdf_path = os.path.join(pydrake.getDrakePath(), "manipulation",
"models", "iiwa_description", "urdf",
"iiwa14_primitive_collision.urdf")
robot_base_frame = RigidBodyFrame("robot_base_frame", rbt.world(),
[0.0, 0.0, 0.0], [0, 0, 0])
AddModelInstanceFromUrdfFile(iiwa_urdf_path, FloatingBaseType.kFixed,
robot_base_frame, rbt)
def setup_scene(rbt):
AddFlatTerrainToWorld(rbt)
instances = []
num_objects = np.random.randint(10) + 1
for i in range(num_objects):
object_ind = np.random.randint(len(objects.keys()))
pose = np.zeros(6)
pose[0:2] = (np.random.random(2) - 0.5) * 0.05
pose[2] = np.random.random() * 0.1 + 0.05
pose[3:6] = np.random.random(3) * np.pi * 2.
object_init_frame = RigidBodyFrame("object_init_frame", rbt.world(),
pose[0:3], pose[3:6])
object_path = objects[objects.keys()[object_ind]]["model_path"]
if object_path.split(".")[-1] == "urdf":
AddModelInstanceFromUrdfFile(object_path,
FloatingBaseType.kRollPitchYaw,
object_init_frame, rbt)
else:
AddModelInstancesFromSdfString(
open(object_path).read(), FloatingBaseType.kRollPitchYaw,
object_init_frame, rbt)
instances.append({
"class": objects.keys()[object_ind],
"pose": pose.tolist()
})
rbt.compile()
# Project arrangement to nonpenetration with IK
constraints = []
constraints.append(
ik.MinDistanceConstraint(model=rbt,
min_distance=0.01,
active_bodies_idx=list(),
active_group_names=set()))
for body_i in range(2, 1 + num_objects):
constraints.append(
ik.WorldPositionConstraint(model=rbt,
body=body_i,
pts=np.array([0., 0., 0.]),
lb=np.array([-0.5, -0.5, 0.0]),
ub=np.array([0.5, 0.5, 0.5])))
q0 = np.zeros(rbt.get_num_positions())
options = ik.IKoptions(rbt)
options.setDebug(True)
options.setMajorIterationsLimit(10000)
options.setIterationsLimit(100000)
results = ik.InverseKin(rbt, q0, q0, constraints, options)
qf = results.q_sol[0]
info = results.info[0]
print "Projected with info %d" % info
return qf, instances
def construct_iiwa_simple():
import os
import pydrake
from pydrake.all import AddFlatTerrainToWorld, RigidBodyFrame, AddModelInstanceFromUrdfFile, FloatingBaseType
# The rigid body tree
rbt = RigidBodyTree()
AddFlatTerrainToWorld(rbt)
# The iiwa
iiwa_urdf_path = os.path.join(
pydrake.getDrakePath(),
"manipulation", "models", "iiwa_description", "urdf",
"iiwa14_polytope_collision.urdf")
robot_base_frame = RigidBodyFrame(
"robot_base_frame", rbt.world(),
[0.0, 0.0, 0.0], [0, 0, 0])
AddModelInstanceFromUrdfFile(iiwa_urdf_path, FloatingBaseType.kFixed,
robot_base_frame, rbt)
return rbt
def getInterp(x,i,debug=0):
x_min = x[np.mod(i,200)]
x_max = x[np.mod(i+1,200)]
if debug:
print np.mod(i,200), x_min
print np.mod(i+1,200), x_max
delta = x_max - x_min
ret = np.array([x_min,x_min+delta/5.0,x_min+2.0*delta/5.0,x_min+3.0*delta/5.0,x_min+4.0*delta/5.0])
if debug:
print ret
return ret
# get the Kuka arm
tree = RigidBodyTree()
world_frame = RigidBodyFrame("world_frame", tree.world(), [0, 0, 0], [0, 0, 0])
path = os.path.join(getDrakePath(),"manipulation","models","iiwa_description","urdf","iiwa14_no_collision.urdf")
AddModelInstanceFromUrdfFile(path, FloatingBaseType.kFixed, world_frame, tree)
# get the ee point and link ID
ee_idx = tree.FindBodyIndex("iiwa_link_7")
body_pt = [0, 0, 0.0635]
# get the 1000 point eeGoals
x = np.array([0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004,0.6556285000000004])
y = np.array([0.13686922001827645,0.1281183229143938,0.11926059413541247,0.11030432312999372,0.1012577993467988,0.09212931223448896,0.08292715124172548,0.0736596058171696,0.06433496540948258,0.05496151946732569,0.045547557439360176,0.03610136877424733,0.026631242920648335,0.017145469327224508,0.007652337442637103,-0.001839863284452653,-0.011322843405383448,-0.020788313471494096,-0.030227984034123273,-0.039633565644609764,-0.0489967688542923,-0.05830930421450961,-0.06756288227660043,-0.07674921359190354,-0.08586000871175764,-0.09488697818749958,-0.1038655258329727,-0.11272375591745916,-0.12145267616480321,-0.13004109492853494,-0.13848752180117063,-0.1467858524541864,-0.1549281455978588,-0.1629165235905652,-0.1707482796892309,-0.17841599130217783,-0.18591882035832843,-0.1932502389635062,-0.20040076105913676,-0.20735798772338723,-0.21411519853223893,-0.22066551450768562,-0.2269966386554578,-0.2331053523296066,-0.23897989082424964,-0.24461177603116246,-0.24999586174091798,-0.255127737887603,-0.2600037985005273,-0.264623973362925,-0.2689800086072113,-0.27306787696210033,-0.2768840041676995,-0.2804279779836409,-0.28369462501981374,-0.28667669709392635,-0.2893761036490012,-0.29178950599107006,-0.29391158123626243,-0.29574034972478985,-0.29727453396452347,-0.29851358128156347,-0.2994544451742904,-0.30009482660988346,-0.30043907902783173,-0.30048644363405164,-0.3002369544043975,-0.29968819678814346,-0.2988444313574429,-0.2977048064508765,-0.2962719042333858,-0.29454317198576974,-0.2925200231035754,-0.2902045731500091,-0.2876022809123594,-0.2847133796914938,-0.28154140071122247,-0.27809048676073667,-0.27436270583704647,-0.2703665745543449,-0.26610524813698555,-0.26158238012185503,-0.25680474076422527,-0.2517742032662073,-0.2464935808285531,-0.24096598110942027,-0.23519855715870805,-0.22919065210989617,-0.22294815964121067,-0.216477551020404,-0.20978904844956445,-0.20289009717093992,-0.19578529751522875,-0.18849136560284127,-0.1810102720333381,-0.17335332623615154,-0.16552932116242403,-0.15754446533330008,-0.1494055335341655,-0.141119776212369,-0.13269158757842503,-0.12412621123921029,-0.11543344583116043,-0.10661783052843572,-0.09768974155699829,-0.08866263788534111,-0.07955032956578706,-0.07036309089673469,-0.06111613853088287,-0.051824258760106816,-0.04250384833178472,-0.03316965050170948,-0.02383739609051122,-0.014517898480146497,-0.00521285872839692,0.004084069854356758,0.013377716369566438,0.022678964415954628,0.03199791841264746,0.04134623887290685,0.05072829056150665,0.06014998301207159,0.06960936374549687,0.07908892160637689,0.08855068744478754,0.09794356384062872,0.10720878615596977,0.11629326145552335,0.12514991959047528,0.13374545516319478,0.14207020091055017,0.15012393938707272,0.15792746608076888,0.16551126865840357,0.1729042968130615,0.18013571994835326,0.1872239892183252,0.19418299168723718,0.20100573965434845,0.2076839316858779,0.2142059797503866,0.22054968628960595,0.22669845070103078,0.2326348093954518,0.2383522458039774,0.24384287073319427,0.2491022174907651,0.2541293098371375,0.2589202699947854,0.2634683865999179,0.2677700650071298,0.27181899023759926,0.2756002672767167,0.2791048721375289,0.28231577838682104,0.28522354073704403,0.2878230537497963,0.2901076694828819,0.2920783437903407,0.29374310403406084,0.295107993386028,0.2961895263765827,0.2970056784978085,0.2975819271104611,0.29794495005548816,0.298109264082986,0.2980740741579594,0.2978323971097816,0.2973591836734637,0.296622924641549,0.2955794967043364,0.29418154658089085,0.2923728011959445,0.2900962060295761,0.2873037962354698,0.28396753800504204,0.28032650994085717,0.2763886650341475,0.27216195627614015,0.26765433665806226,0.2628737591711411,0.2578281768066035,0.25252554255567694,0.2469738094095883,0.2411809303595648,0.2351548583968336,0.2289035465126218,0.22243494769815644,0.21575701494466482,0.208877701243374,0.201804959585511,0.19454674296230312,0.1871110043649775,0.179505696784761,0.1717387732128811,0.16381818664056474,0.1557518900590391,0.14754783645953137,0.1392139788332685,0.1307582701714778])
z = np.array([0.31756840547539744,0.32748579299353975,0.3378005717933035,0.3484792783644661,0.3594884491968049,0.3707946207800972,0.38236432960412026,0.39416411215865155,0.40616050493346834,0.4183200444183479,0.4306092671030677,0.44299470947740494,0.455442908031137,0.4679203992540413,0.48039371963589506,0.49282940566647565,0.5051939938355604,0.5174540206329267,0.5295760225483518,0.541526536071613,0.5532720976924878,0.5647792439007534,0.5760145111861872,0.5869444360385665,0.5975355549476685,0.607754404403259,0.617684862624283,0.6269833110602612,0.6356398015809976,0.6436439919363848,0.6509877754875409,0.6576613603932012,0.6636610682008525,0.6689826360732843,0.67361769235995,0.6775587102765505,0.6807894425694675,0.6832982853518634,0.6850685995152646,0.6860878343790213,0.6863475050397387,0.6858378423067206,0.6845542073433015,0.6824970237151333,0.679674950734998,0.6760903644476528,0.6717601780334646,0.6666986364356934,0.6609289904640975,0.6544774785385726,0.6473731907857357,0.6396420054814256,0.6313175949625012,0.6224353167682047,0.6130293234274724,0.6031408593626014,0.592806201725962,0.5820708170060365,0.5709740379170426,0.5595578699404182,0.5478698928627949,0.5359537973679934,0.5238581689279385,0.5116302430406027,0.4993208702348768,0.48697884889804044,0.4746532265736393,0.4623956012593628,0.4502485492310045,0.4382596910462214,0.42647866542842733,0.41494764320821875,0.4037154159777041,0.39282851027202587,0.3823335341683771,0.37227688282859595,0.36269856168881953,0.3536372194160615,0.3451304536909037,0.33721132075471055,0.32990808625336293,0.32324779496703726,0.3172587080115004,0.31196097533352374,0.3073723368592661,0.3035111825862362,0.3003931753833552,0.29803437903462704,0.2964420439874006,0.29562574916758594,0.2955914743255737,0.29633569503272444,0.29786307504133164,0.3001661781693752,0.30323465990169074,0.3070520940451594,0.3115998595664622,0.31685456748736623,0.3227967201975068,0.32939373258736826,0.336620401368088,0.34444137126548074,0.35282689981652365,0.3617486647489115,0.37117885795827055,0.3810878162585716,0.39144908831456365,0.4022280096944361,0.41338530657545247,0.42488278556704895,0.43667841400710394,0.4487240639652002,0.46096913859882693,0.47336419850958344,0.48584480962195276,0.4983474665337738,0.5108078008561817,0.5231640943169621,0.5353543092708679,0.5473260617454424,0.5590277695983947,0.5704169154454125,0.5814512174681081,0.5920942512854084,0.6023199392964617,0.6120999705542476,0.6214094905886763,0.6302189177557694,0.6384948605857313,0.6462095585403849,0.6533240617001391,0.6598152959296799,0.6656559786178493,0.6708180864798622,0.6752789417653723,0.6790134023420558,0.6819951301275096,0.6841972139799184,0.6856012933475145,0.6861863032005122,0.6859428344922742,0.6848659833744687,0.6829691340687087,0.6802671521438632,0.6767869940428872,0.6725559197263683,0.6675988289655357,0.6619506036376687,0.6556389703050904,0.6486908922059266,0.6411394935332501,0.6330153548211616,0.6243554553896911,0.6151955559582207,0.6055661079526033,0.5954998822170263,0.5850395750753018,0.5742180207932119,0.563083467349193,0.5516783030193103,0.5400466850891197,0.5282336225055959,0.5162846587691582,0.5042482411832561,0.49216898910506657,0.4800977462683095,0.4680759949262634,0.45614115835012375,0.4443280391401759,0.4326669709393109,0.42118572108087715,0.40991017010577024,0.3988536909103188,0.3880358943577357,0.37746325851094437,0.3677153816185399,0.3583626678734707,0.34943548878219216,0.34096421585115233,0.33297922058679913,0.3255108744955808,0.3185895490839454,0.31224561585834093,0.30650944632521565,0.30141141199101745,0.2969818843621946,0.29325123494519506,0.29024983524646697,0.28800805677245844,0.2865562710296175,0.28592484952439223,0.2861441637632308,0.28724458525258123,0.2892564854988917,0.2922102360086102,0.2961362082881848,0.30106477384406377,0.30702630418269494,0.3140511708105266,0.32216974523400665])
newx = np.array([])
newy = np.array([])
newz = np.array([])
for i in range(200):
hand_controller.robot_state_input_port)
builder.Connect(manip_state_machine.hand_setpoint_output_port,
hand_controller.setpoint_input_port)
builder.Connect(hand_controller.get_output_port(0),
rbplant_sys.get_input_port(1))
# Hook up the visualizer we created earlier.
visualizer = builder.AddSystem(pbrv)
builder.Connect(rbplant_sys.state_output_port(),
visualizer.get_input_port(0))
# Add a camera, too, though no controller or estimator
# will consume the output of it.
# - Add frame for camera fixture.
camera_frame = RigidBodyFrame(
name="rgbd camera frame", body=rbt.world(),
xyz=[2, 0., 1.5], rpy=[-np.pi/4, 0., -np.pi])
rbt.addFrame(camera_frame)
camera = builder.AddSystem(
RgbdCamera(name="camera", tree=rbt, frame=camera_frame,
z_near=0.5, z_far=2.0, fov_y=np.pi / 4,
width=320, height=240,
show_window=False))
builder.Connect(rbplant_sys.state_output_port(),
camera.get_input_port(0))
camera_meshcat_visualizer = builder.AddSystem(
kuka_utils.RgbdCameraMeshcatVisualizer(camera, rbt))
builder.Connect(camera.depth_image_output_port(),
camera_meshcat_visualizer.camera_input_port)
builder.Connect(rbplant_sys.state_output_port(),
def addKukaSetup(tree, collision_type="none"):
world_frame = RigidBodyFrame("world_frame", tree.world(), [0, 0, 0],
[0, 0, 0])
kuka_urdf_path = getKukaUrdfPath(collision_type=collision_type)
AddModelInstanceFromUrdfFile(kuka_urdf_path, FloatingBaseType.kFixed,
world_frame, tree)
def get_figure_eight_traj_simple():
tree = RigidBodyTree()
world_frame = RigidBodyFrame("world_frame", tree.world(), [0, 0, 0],
[0, 0, 0])
kuka_urdf_path = getKukaUrdfPath()
AddModelInstanceFromUrdfFile(kuka_urdf_path, FloatingBaseType.kFixed,
world_frame, tree)
Nq = tree.get_num_positions()
eps = 1e-5
ee_idx = tree.FindBodyIndex("iiwa_link_7")
body_pt = [0, 0, 0.0635]
x = np.array([
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004
])
y = np.array([
0.13686922001827645, 0.1281183229143938, 0.11926059413541247,
0.11030432312999372, 0.1012577993467988, 0.09212931223448896,
0.08292715124172548, 0.0736596058171696, 0.06433496540948258,
0.05496151946732569, 0.045547557439360176, 0.03610136877424733,
0.026631242920648335, 0.017145469327224508, 0.007652337442637103,
-0.001839863284452653, -0.011322843405383448, -0.020788313471494096,
-0.030227984034123273, -0.039633565644609764, -0.0489967688542923,
-0.05830930421450961, -0.06756288227660043, -0.07674921359190354,
-0.08586000871175764, -0.09488697818749958, -0.1038655258329727,
-0.11272375591745916, -0.12145267616480321, -0.13004109492853494,
-0.13848752180117063, -0.1467858524541864, -0.1549281455978588,
-0.1629165235905652, -0.1707482796892309, -0.17841599130217783,
-0.18591882035832843, -0.1932502389635062, -0.20040076105913676,
-0.20735798772338723, -0.21411519853223893, -0.22066551450768562,
-0.2269966386554578, -0.2331053523296066, -0.23897989082424964,
-0.24461177603116246, -0.24999586174091798, -0.255127737887603,
-0.2600037985005273, -0.264623973362925, -0.2689800086072113,
-0.27306787696210033, -0.2768840041676995, -0.2804279779836409,
-0.28369462501981374, -0.28667669709392635, -0.2893761036490012,
-0.29178950599107006, -0.29391158123626243, -0.29574034972478985,
-0.29727453396452347, -0.29851358128156347, -0.2994544451742904,
-0.30009482660988346, -0.30043907902783173, -0.30048644363405164,
-0.3002369544043975, -0.29968819678814346, -0.2988444313574429,
-0.2977048064508765, -0.2962719042333858, -0.29454317198576974,
-0.2925200231035754, -0.2902045731500091, -0.2876022809123594,
-0.2847133796914938, -0.28154140071122247, -0.27809048676073667,
-0.27436270583704647, -0.2703665745543449, -0.26610524813698555,
-0.26158238012185503, -0.25680474076422527, -0.2517742032662073,
-0.2464935808285531, -0.24096598110942027, -0.23519855715870805,
-0.22919065210989617, -0.22294815964121067, -0.216477551020404,
-0.20978904844956445, -0.20289009717093992, -0.19578529751522875,
-0.18849136560284127, -0.1810102720333381, -0.17335332623615154,
-0.16552932116242403, -0.15754446533330008, -0.1494055335341655,
-0.141119776212369, -0.13269158757842503, -0.12412621123921029,
-0.11543344583116043, -0.10661783052843572, -0.09768974155699829,
-0.08866263788534111, -0.07955032956578706, -0.07036309089673469,
-0.06111613853088287, -0.051824258760106816, -0.04250384833178472,
-0.03316965050170948, -0.02383739609051122, -0.014517898480146497,
-0.00521285872839692, 0.004084069854356758, 0.013377716369566438,
0.022678964415954628, 0.03199791841264746, 0.04134623887290685,
0.05072829056150665, 0.06014998301207159, 0.06960936374549687,
0.07908892160637689, 0.08855068744478754, 0.09794356384062872,
0.10720878615596977, 0.11629326145552335, 0.12514991959047528,
0.13374545516319478, 0.14207020091055017, 0.15012393938707272,
0.15792746608076888, 0.16551126865840357, 0.1729042968130615,
0.18013571994835326, 0.1872239892183252, 0.19418299168723718,
0.20100573965434845, 0.2076839316858779, 0.2142059797503866,
0.22054968628960595, 0.22669845070103078, 0.2326348093954518,
0.2383522458039774, 0.24384287073319427, 0.2491022174907651,
0.2541293098371375, 0.2589202699947854, 0.2634683865999179,
0.2677700650071298, 0.27181899023759926, 0.2756002672767167,
0.2791048721375289, 0.28231577838682104, 0.28522354073704403,
0.2878230537497963, 0.2901076694828819, 0.2920783437903407,
0.29374310403406084, 0.295107993386028, 0.2961895263765827,
0.2970056784978085, 0.2975819271104611, 0.29794495005548816,
0.298109264082986, 0.2980740741579594, 0.2978323971097816,
0.2973591836734637, 0.296622924641549, 0.2955794967043364,
0.29418154658089085, 0.2923728011959445, 0.2900962060295761,
0.2873037962354698, 0.28396753800504204, 0.28032650994085717,
0.2763886650341475, 0.27216195627614015, 0.26765433665806226,
0.2628737591711411, 0.2578281768066035, 0.25252554255567694,
0.2469738094095883, 0.2411809303595648, 0.2351548583968336,
0.2289035465126218, 0.22243494769815644, 0.21575701494466482,
0.208877701243374, 0.201804959585511, 0.19454674296230312,
0.1871110043649775, 0.179505696784761, 0.1717387732128811,
0.16381818664056474, 0.1557518900590391, 0.14754783645953137,
0.1392139788332685, 0.1307582701714778
])
z = np.array([
0.31756840547539744, 0.32748579299353975, 0.3378005717933035,
0.3484792783644661, 0.3594884491968049, 0.3707946207800972,
0.38236432960412026, 0.39416411215865155, 0.40616050493346834,
0.4183200444183479, 0.4306092671030677, 0.44299470947740494,
0.455442908031137, 0.4679203992540413, 0.48039371963589506,
0.49282940566647565, 0.5051939938355604, 0.5174540206329267,
0.5295760225483518, 0.541526536071613, 0.5532720976924878,
0.5647792439007534, 0.5760145111861872, 0.5869444360385665,
0.5975355549476685, 0.607754404403259, 0.617684862624283,
0.6269833110602612, 0.6356398015809976, 0.6436439919363848,
0.6509877754875409, 0.6576613603932012, 0.6636610682008525,
0.6689826360732843, 0.67361769235995, 0.6775587102765505,
0.6807894425694675, 0.6832982853518634, 0.6850685995152646,
0.6860878343790213, 0.6863475050397387, 0.6858378423067206,
0.6845542073433015, 0.6824970237151333, 0.679674950734998,
0.6760903644476528, 0.6717601780334646, 0.6666986364356934,
0.6609289904640975, 0.6544774785385726, 0.6473731907857357,
0.6396420054814256, 0.6313175949625012, 0.6224353167682047,
0.6130293234274724, 0.6031408593626014, 0.592806201725962,
0.5820708170060365, 0.5709740379170426, 0.5595578699404182,
0.5478698928627949, 0.5359537973679934, 0.5238581689279385,
0.5116302430406027, 0.4993208702348768, 0.48697884889804044,
0.4746532265736393, 0.4623956012593628, 0.4502485492310045,
0.4382596910462214, 0.42647866542842733, 0.41494764320821875,
0.4037154159777041, 0.39282851027202587, 0.3823335341683771,
0.37227688282859595, 0.36269856168881953, 0.3536372194160615,
0.3451304536909037, 0.33721132075471055, 0.32990808625336293,
0.32324779496703726, 0.3172587080115004, 0.31196097533352374,
0.3073723368592661, 0.3035111825862362, 0.3003931753833552,
0.29803437903462704, 0.2964420439874006, 0.29562574916758594,
0.2955914743255737, 0.29633569503272444, 0.29786307504133164,
0.3001661781693752, 0.30323465990169074, 0.3070520940451594,
0.3115998595664622, 0.31685456748736623, 0.3227967201975068,
0.32939373258736826, 0.336620401368088, 0.34444137126548074,
0.35282689981652365, 0.3617486647489115, 0.37117885795827055,
0.3810878162585716, 0.39144908831456365, 0.4022280096944361,
0.41338530657545247, 0.42488278556704895, 0.43667841400710394,
0.4487240639652002, 0.46096913859882693, 0.47336419850958344,
0.48584480962195276, 0.4983474665337738, 0.5108078008561817,
0.5231640943169621, 0.5353543092708679, 0.5473260617454424,
0.5590277695983947, 0.5704169154454125, 0.5814512174681081,
0.5920942512854084, 0.6023199392964617, 0.6120999705542476,
0.6214094905886763, 0.6302189177557694, 0.6384948605857313,
0.6462095585403849, 0.6533240617001391, 0.6598152959296799,
0.6656559786178493, 0.6708180864798622, 0.6752789417653723,
0.6790134023420558, 0.6819951301275096, 0.6841972139799184,
0.6856012933475145, 0.6861863032005122, 0.6859428344922742,
0.6848659833744687, 0.6829691340687087, 0.6802671521438632,
0.6767869940428872, 0.6725559197263683, 0.6675988289655357,
0.6619506036376687, 0.6556389703050904, 0.6486908922059266,
0.6411394935332501, 0.6330153548211616, 0.6243554553896911,
0.6151955559582207, 0.6055661079526033, 0.5954998822170263,
0.5850395750753018, 0.5742180207932119, 0.563083467349193,
0.5516783030193103, 0.5400466850891197, 0.5282336225055959,
0.5162846587691582, 0.5042482411832561, 0.49216898910506657,
0.4800977462683095, 0.4680759949262634, 0.45614115835012375,
0.4443280391401759, 0.4326669709393109, 0.42118572108087715,
0.40991017010577024, 0.3988536909103188, 0.3880358943577357,
0.37746325851094437, 0.3677153816185399, 0.3583626678734707,
0.34943548878219216, 0.34096421585115233, 0.33297922058679913,
0.3255108744955808, 0.3185895490839454, 0.31224561585834093,
0.30650944632521565, 0.30141141199101745, 0.2969818843621946,
0.29325123494519506, 0.29024983524646697, 0.28800805677245844,
0.2865562710296175, 0.28592484952439223, 0.2861441637632308,
0.28724458525258123, 0.2892564854988917, 0.2922102360086102,
0.2961362082881848, 0.30106477384406377, 0.30702630418269494,
0.3140511708105266, 0.32216974523400665
])
newx = np.array([])
newy = np.array([])
newz = np.array([])
for i in range(200):
newx = np.concatenate((newx, getInterp(x, i)))
newy = np.concatenate((newy, getInterp(y, i)))
newz = np.concatenate((newz, getInterp(z, i)))
# xyz = np.vstack((x, y, z))
xyz = np.vstack((newx, newy, newz))
# compute inverse kinematics
ik_options = IKoptions(tree)
ik_options.setDebug(1)
q_seed = np.array([0, np.pi / 4, 0, -np.pi / 4, 0, -np.pi / 4, 0])
print q_seed
for i in range(len(x)):
ee_des = xyz[:, i]
cnstr = WorldPositionConstraint(tree, ee_idx, body_pt, ee_des - eps,
ee_des + eps)
res = InverseKin(tree, q_seed, q_seed, [cnstr], ik_options)
print res.q_sol[0]
print res.info
kinsol = tree.doKinematics(res.q_sol[0])
ee_idx = tree.FindBodyIndex("iiwa_link_7")
print tree.transformPoints(kinsol, np.zeros(3), 0, ee_idx)
break