def experimental_inverse_kinematics(robot,
link,
pose,
null_space=False,
max_iterations=200,
tolerance=1e-3):
(point, quat) = pose
# https://github.com/bulletphysics/bullet3/blob/389d7aaa798e5564028ce75091a3eac6a5f76ea8/examples/SharedMemory/PhysicsClientC_API.cpp
# https://github.com/bulletphysics/bullet3/blob/c1ba04a5809f7831fa2dee684d6747951a5da602/examples/pybullet/examples/inverse_kinematics_husky_kuka.py
joints = get_joints(
robot) # Need to have all joints (although only movable returned)
movable_joints = get_movable_joints(robot)
current_conf = get_joint_positions(robot, joints)
# TODO: sample other values for the arm joints as the reference conf
min_limits = [get_joint_limits(robot, joint)[0] for joint in joints]
max_limits = [get_joint_limits(robot, joint)[1] for joint in joints]
#min_limits = current_conf
#max_limits = current_conf
#max_velocities = [get_max_velocity(robot, joint) for joint in joints] # Range of Jacobian
max_velocities = [10000] * len(joints)
# TODO: cannot have zero velocities
# TODO: larger definitely better for velocities
#damping = tuple(0.1*np.ones(len(joints)))
#t0 = time.time()
#kinematic_conf = get_joint_positions(robot, movable_joints)
for iterations in range(max_iterations): # 0.000863273143768 / iteration
# TODO: return none if no progress
if null_space:
kinematic_conf = p.calculateInverseKinematics(
robot,
link,
point,
quat,
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=max_velocities,
restPoses=current_conf,
#jointDamping=damping,
)
else:
kinematic_conf = p.calculateInverseKinematics(
robot, link, point, quat)
if (kinematic_conf is None) or any(map(math.isnan, kinematic_conf)):
return None
set_joint_positions(robot, movable_joints, kinematic_conf)
link_point, link_quat = get_link_pose(robot, link)
if np.allclose(link_point, point, atol=tolerance) and np.allclose(
link_quat, quat, atol=tolerance):
#print(iterations)
break
else:
return None
if violates_limits(robot, movable_joints, kinematic_conf):
return None
#total_time = (time.time() - t0)
#print(total_time)
#print((time.time() - t0)/max_iterations)
return kinematic_conf
def fn(robot_name, conf1, conf2, fluents=[]):
robot = task.mbp.GetModelInstanceByName(robot_name)
joints = get_movable_joints(task.mbp, robot)
moving = bodies_from_models(task.mbp, [robot, gripper])
obstacles = set(task.fixed_bodies())
attachments = parse_fluents(fluents, context, obstacles)
for grasp in attachments:
moving.update(grasp.bodies)
obstacles -= moving
collision_pairs = set(product(moving,
obstacles)) if collisions else set()
collision_fn = get_collision_fn(task.diagram,
task.diagram_context,
task.mbp,
task.scene_graph,
joints,
collision_pairs=collision_pairs,
attachments=attachments)
open_wsg50_gripper(task.mbp, context, gripper)
path = plan_joint_motion(joints,
conf1.positions,
conf2.positions,
collision_fn=collision_fn,
restarts=10,
iterations=75,
smooth=50)
if path is None:
return None
traj = Trajectory([Conf(joints, q) for q in path],
attachments=attachments)
return (traj, )
def get_open_trajectory(plant, gripper):
gripper_joints = get_movable_joints(plant, gripper)
gripper_extend_fn = get_extend_fn(gripper_joints)
gripper_closed_conf = get_close_wsg50_positions(plant, gripper)
gripper_path = list(
gripper_extend_fn(gripper_closed_conf,
get_open_wsg50_positions(plant, gripper)))
gripper_path.insert(0, gripper_closed_conf)
return Trajectory(Conf(gripper_joints, q) for q in gripper_path)
def fn(robot_name, obj_name, obj_pose, obj_grasp):
# TODO: if gripper/block in collision, return
robot = task.mbp.GetModelInstanceByName(robot_name)
joints = get_movable_joints(task.mbp, robot)
collision_pairs = set(
product(bodies_from_models(task.mbp, [robot, task.gripper]),
fixed))
collision_fn = get_collision_fn(
task.diagram,
task.diagram_context,
task.mbp,
task.scene_graph,
joints,
collision_pairs=collision_pairs) # TODO: while holding
gripper_pose = obj_pose.transform.multiply(
obj_grasp.transform.inverse())
gripper_path = list(
interpolate_translation(gripper_pose,
approach_vector,
step_size=step_size))
attempts = 0
last_success = 0
while (attempts - last_success) < max_failures:
attempts += 1
obj_pose.assign(context)
path = plan_frame_motion(task.plant,
joints,
gripper_frame,
gripper_path,
initial_guess=initial_guess,
collision_fn=collision_fn)
if path is None:
continue
traj = Trajectory([Conf(joints, q) for q in path],
attachments=[obj_grasp])
conf = traj.path[-1]
yield (conf, traj)
last_success = attempts
def load_station(time_step=0.0, plan=None):
# TODO: map names to SDF paths
# https://github.com/RobotLocomotion/drake/blob/master/bindings/pydrake/examples/manipulation_station_py.cc
#object_file_path = FindResourceOrThrow(
# "drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf")
object_file_path = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
# RuntimeError: Error control wants to select step smaller than minimum allowed (1e-14)
station = ManipulationStation(time_step, IiwaCollisionModel.kBoxCollision)
plant = station.get_mutable_multibody_plant()
scene_graph = station.get_mutable_scene_graph()
robot = plant.GetModelInstanceByName('iiwa')
gripper = plant.GetModelInstanceByName('gripper')
station.AddCupboard()
brick = AddModelFromSdfFile(file_name=object_file_path,
model_name="brick",
plant=plant,
scene_graph=scene_graph)
station.Finalize()
door_names = ['left_door', 'right_door']
doors = [plant.GetBodyByName(name).index() for name in door_names]
initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0]
#initial_conf[1] += np.pi / 6
initial_positions = {
plant.GetJointByName('left_door_hinge'):
-DOOR_CLOSED,
#plant.GetJointByName('left_door_hinge'): -np.pi / 2,
plant.GetJointByName('right_door_hinge'):
np.pi / 2,
#plant.GetJointByName('right_door_hinge'): np.pi,
}
initial_positions.update(
zip(get_movable_joints(plant, robot), initial_conf))
initial_poses = {
brick:
create_transform(translation=[0.3, 0, 0], rotation=[0, 0, np.pi / 2]),
}
goal_poses = {
brick:
create_transform(translation=[0.8, 0.2, 0.2927],
rotation=[0, 0, 5 * np.pi / 4]),
}
diagram, state_machine = build_manipulation_station(station, plan)
task = Task(diagram,
plant,
scene_graph,
robot,
gripper,
movable=[brick],
doors=doors,
initial_positions=initial_positions,
initial_poses=initial_poses,
goal_poses=goal_poses,
reset_robot=True,
reset_doors=False)
task.set_initial()
return task, diagram, state_machine
def fn(robot_name, door_name, door_conf1, door_conf2):
"""
:param robot_name: The name of the robot (should be iiwa)
:param door_name: The name of the door (should be left_door or right_door)
:param door_conf1: The initial door configuration
:param door_conf2: The final door configuration
:return: A triplet composed of the initial robot configuration, final robot configuration,
and combined robot & door position trajectory to execute the pull
"""
robot = task.mbp.GetModelInstanceByName(robot_name)
robot_joints = get_movable_joints(task.mbp, robot)
collision_pairs = set(
product(bodies_from_models(task.mbp, [robot, task.gripper]),
fixed))
collision_fn = get_collision_fn(task.diagram,
task.diagram_context,
task.mbp,
task.scene_graph,
robot_joints,
collision_pairs=collision_pairs)
door_body = task.mbp.GetBodyByName(door_name)
door_joints = door_conf1.joints
combined_joints = robot_joints + door_joints
# The transformation from the door frame to the gripper frame that corresponds to grasping the door handle
gripper_from_door = get_door_grasp(door_body, box_from_geom)
def get_robot_joints(door_body, door_joint):
door_pose_world = task.plant.tree().EvalBodyPoseInWorld(
context, door_body)
door_pose_gripper = np.dot(gripper_from_door, door_pose_world)
##### BEGIN YOUR CODE HERE #####
# combined_joint_path is a joint position path for the concatenated robot and door joints.
# It should be a list of 8 DOF configurations (7 robot DOFs + 1 door DOF).
# Additionally, combined_joint_path[0][len(robot_joints):] should equal door_conf1.positions
# and combined_joint_path[-1][len(robot_joints):] should equal door_conf2.positions.
num_knots = 4
door_joints_list = [door_joints[0]]
print door_conf1.joints[0].get_angle(context)
print door_conf2.joints[0].get_angle(context)
for i in range(num_knots):
door_joints_list.append(door_joints[0].set_angle(
context, door_joints[0].get_angle(context) +
(door_conf2.joints[0].get_angle(context) -
door_conf1.joints[0].get_angle(context)) *
(i + 1) / num_knots))
print[dj.get_angle(context) for dj in door_joints_list]
# if path is None:
# continue
combined_joint_path = [
get_robot_joints(door_body, dj.get_angle(context))
for dj in door_joints_list
]
##### END YOUR CODE HERE #####
robot_conf1 = Conf(robot_joints,
combined_joint_path[0][:len(robot_joints)])
robot_conf2 = Conf(robot_joints,
combined_joint_path[-1][:len(robot_joints)])
traj = Trajectory(
Conf(combined_joints, combined_conf)
for combined_conf in combined_joint_path)
yield (robot_conf1, robot_conf2, traj)
def get_pddlstream_problem(task, context, collisions=True):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
plant = task.mbp
robot = task.robot
robot_name = get_model_name(plant, robot)
world = plant.world_frame() # mbp.world_body()
robot_joints = get_movable_joints(plant, robot)
robot_conf = Conf(robot_joints, get_configuration(plant, context, robot))
init = [
('Robot', robot_name),
('CanMove', robot_name),
('Conf', robot_name, robot_conf),
('AtConf', robot_name, robot_conf),
('HandEmpty', robot_name),
]
goal_literals = []
if task.reset_robot:
goal_literals.append(('AtConf', robot_name, robot_conf),)
for obj in task.movable:
obj_name = get_model_name(plant, obj)
obj_pose = Pose(plant, world, obj, get_world_pose(plant, context, obj))
init += [('Graspable', obj_name),
('Pose', obj_name, obj_pose),
('AtPose', obj_name, obj_pose)]
for surface in task.surfaces:
init += [('Stackable', obj_name, surface)]
for surface in task.surfaces:
surface_name = get_model_name(plant, surface.model_index)
if 'sink' in surface_name:
init += [('Sink', surface)]
if 'stove' in surface_name:
init += [('Stove', surface)]
for door in task.doors:
door_body = plant.tree().get_body(door)
door_name = door_body.name()
door_joints = get_parent_joints(plant, door_body)
door_conf = Conf(door_joints, get_joint_positions(door_joints, context))
init += [
('Door', door_name),
('Conf', door_name, door_conf),
('AtConf', door_name, door_conf),
]
for positions in [get_door_positions(door_body, DOOR_OPEN)]:
conf = Conf(door_joints, positions)
init += [('Conf', door_name, conf)]
if task.reset_doors:
goal_literals += [('AtConf', door_name, door_conf)]
for obj, transform in task.goal_poses.items():
obj_name = get_model_name(plant, obj)
obj_pose = Pose(plant, world, obj, transform)
init += [('Pose', obj_name, obj_pose)]
goal_literals.append(('AtPose', obj_name, obj_pose))
for obj in task.goal_holding:
goal_literals.append(('Holding', robot_name, get_model_name(plant, obj)))
for obj, surface in task.goal_on:
goal_literals.append(('On', get_model_name(plant, obj), surface))
for obj in task.goal_cooked:
goal_literals.append(('Cooked', get_model_name(plant, obj)))
goal = And(*goal_literals)
print('Initial:', init)
print('Goal:', goal)
stream_map = {
'sample-grasp': from_gen_fn(get_grasp_gen_fn(task)),
'plan-ik': from_gen_fn(get_ik_gen_fn(task, context, collisions=collisions)),
'plan-motion': from_fn(get_motion_fn(task, context, collisions=collisions)),
'plan-pull': from_gen_fn(get_pull_fn(task, context, collisions=collisions)),
'TrajPoseCollision': get_collision_test(task, context, collisions=collisions),
'TrajConfCollision': get_collision_test(task, context, collisions=collisions),
}
#stream_map = 'debug' # Runs PDDLStream with "placeholder streams" for debugging
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def main():
# TODO: teleporting kuka arm
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
client = connect(use_gui=args.viewer)
add_data_path()
#planeId = p.loadURDF("plane.urdf")
table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107)
box = create_box(.07, .05, .15)
# boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
#pr2 = p.loadURDF("pr2_description/pr2.urdf")
pr2 = p.loadURDF("pr2_description/pr2_fixed_torso.urdf")
#pr2 = p.loadURDF("/Users/caelan/Programs/Installation/pr2_drake/pr2_local2.urdf",)
#useFixedBase=0,)
#flags=p.URDF_USE_SELF_COLLISION)
#flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
#flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
#pr2 = p.loadURDF("pr2_drake/urdf/pr2_simplified.urdf", useFixedBase=False)
initially_colliding = get_colliding_links(pr2)
print len(initially_colliding)
origin = (0, 0, 0)
print p.getNumConstraints()
# TODO: no way of controlling the base position by itself
# TODO: PR2 seems to collide with itself frequently
# real_time = False
# p.setRealTimeSimulation(real_time)
# left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
# control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM)
# while True:
# control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM)
# if not real_time:
# p.stepSimulation()
# A CollisionMap robot allows the user to specify self-collision regions indexed by the values of two joints.
# GetRigidlyAttachedLinks
print pr2
# for i in range (10000):
# p.stepSimulation()
# time.sleep(1./240.)
#print get_joint_names(pr2)
print [get_joint_name(pr2, joint) for joint in get_movable_joints(pr2)]
print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME))
#open_gripper(pr2, joint_from_name(pr2, LEFT_GRIPPER))
#print get_joint_limits(pr2, joint_from_name(pr2, LEFT_GRIPPER))
#print get_joint_position(pr2, joint_from_name(pr2, LEFT_GRIPPER))
print self_collision(pr2)
"""
print p.getNumConstraints()
constraint = fixed_constraint(pr2, -1, box, -1) # table
p.changeConstraint(constraint)
print p.getNumConstraints()
print p.getConstraintInfo(constraint)
print p.getConstraintState(constraint)
p.stepSimulation()
raw_input('Continue?')
set_point(pr2, (-2, 0, 0))
p.stepSimulation()
p.changeConstraint(constraint)
print p.getConstraintInfo(constraint)
print p.getConstraintState(constraint)
raw_input('Continue?')
print get_point(pr2)
raw_input('Continue?')
"""
# TODO: would be good if we could set the joint directly
print set_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME), 0.2) # Updates automatically
print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME))
#return
left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
right_joints = [joint_from_name(pr2, name) for name in RIGHT_JOINT_NAMES]
print set_joint_positions(pr2, left_joints, TOP_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM
print set_joint_positions(pr2, right_joints, REST_RIGHT_ARM) # TOP_HOLDING_RIGHT_ARM | REST_RIGHT_ARM
print get_body_name(pr2)
print get_body_names()
# print p.getBodyUniqueId(pr2)
print get_joint_names(pr2)
#for joint, value in zip(LEFT_ARM_JOINTS, REST_LEFT_ARM):
# set_joint_position(pr2, joint, value)
# for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM):
# joint = joint_from_name(pr2, name)
# #print name, joint, get_joint_position(pr2, joint), value
# print name, get_joint_limits(pr2, joint), get_joint_type(pr2, joint), get_link_name(pr2, joint)
# set_joint_position(pr2, joint, value)
# #print name, joint, get_joint_position(pr2, joint), value
# for name, value in zip(RIGHT_JOINT_NAMES, REST_RIGHT_ARM):
# set_joint_position(pr2, joint_from_name(pr2, name), value)
print p.getNumJoints(pr2)
jointId = 0
print p.getJointInfo(pr2, jointId)
print p.getJointState(pr2, jointId)
# for i in xrange(10):
# #lower, upper = BASE_LIMITS
# #q = np.random.rand(len(lower))*(np.array(upper) - np.array(lower)) + lower
# q = np.random.uniform(*BASE_LIMITS)
# theta = np.random.uniform(*REVOLUTE_LIMITS)
# quat = z_rotation(theta)
# print q, theta, quat, env_collision(pr2)
# #set_point(pr2, q)
# set_pose(pr2, q, quat)
# #p.getMouseEvents()
# #p.getKeyboardEvents()
# raw_input('Continue?') # Stalls because waiting for input
#
# # TODO: self collisions
# for i in xrange(10):
# for name in LEFT_JOINT_NAMES:
# joint = joint_from_name(pr2, name)
# value = np.random.uniform(*get_joint_limits(pr2, joint))
# set_joint_position(pr2, joint, value)
# raw_input('Continue?')
#start = (-2, -2, 0)
#set_base_values(pr2, start)
# #start = get_base_values(pr2)
# goal = (2, 2, 0)
# p.addUserDebugLine(start, goal, lineColorRGB=(1, 1, 0)) # addUserDebugText
# print start, goal
# raw_input('Plan?')
# path = plan_base_motion(pr2, goal)
# print path
# if path is None:
# return
# print len(path)
# for bq in path:
# set_base_values(pr2, bq)
# raw_input('Continue?')
# left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
# for joint in left_joints:
# print joint, get_joint_name(pr2, joint), get_joint_limits(pr2, joint), \
# is_circular(pr2, joint), get_joint_position(pr2, joint)
#
# #goal = np.zeros(len(left_joints))
# goal = []
# for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM):
# joint = joint_from_name(pr2, name)
# goal.append(wrap_joint(pr2, joint, value))
#
# path = plan_joint_motion(pr2, left_joints, goal)
# print path
# for q in path:s
# set_joint_positions(pr2, left_joints, q)
# raw_input('Continue?')
print p.JOINT_REVOLUTE, p.JOINT_PRISMATIC, p.JOINT_FIXED, p.JOINT_POINT2POINT, p.JOINT_GEAR # 0 1 4 5 6
movable_joints = get_movable_joints(pr2)
print len(movable_joints)
for joint in xrange(get_num_joints(pr2)):
if is_movable(pr2, joint):
print joint, get_joint_name(pr2, joint), get_joint_type(pr2, joint), get_joint_limits(pr2, joint)
#joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
#set_joint_positions(pr2, joints, sample_joints(pr2, joints))
#print get_joint_positions(pr2, joints) # Need to print before the display updates?
# set_base_values(pr2, (1, -1, -np.pi/4))
# movable_joints = get_movable_joints(pr2)
# gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK))
# print gripper_pose
# print get_joint_positions(pr2, movable_joints)
# p.addUserDebugLine(origin, gripper_pose[0], lineColorRGB=(1, 0, 0))
# p.stepSimulation()
# raw_input('Pre2 IK')
# set_joint_positions(pr2, left_joints, SIDE_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM
# print get_joint_positions(pr2, movable_joints)
# p.stepSimulation()
# raw_input('Pre IK')
# conf = inverse_kinematics(pr2, gripper_pose) # Doesn't automatically set configuraitons
# print conf
# print get_joint_positions(pr2, movable_joints)
# set_joint_positions(pr2, movable_joints, conf)
# print get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK))
# #print get_joint_positions(pr2, movable_joints)
# p.stepSimulation()
# raw_input('Post IK')
# return
# print pose_from_tform(TOOL_TFORM)
# gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK))
# #gripper_pose = multiply(gripper_pose, TOOL_POSE)
# #gripper_pose = get_gripper_pose(pr2)
# for i, grasp_pose in enumerate(get_top_grasps(box)):
# grasp_pose = multiply(TOOL_POSE, grasp_pose)
# box_pose = multiply(gripper_pose, grasp_pose)
# set_pose(box, *box_pose)
# print get_pose(box)
# raw_input('Grasp {}'.format(i))
# return
torso = joint_from_name(pr2, TORSO_JOINT_NAME)
torso_point, torso_quat = get_link_pose(pr2, torso)
#torso_constraint = p.createConstraint(pr2, torso, -1, -1,
# p.JOINT_FIXED, jointAxis=[0] * 3, # JOINT_FIXED
# parentFramePosition=torso_point,
# childFramePosition=torso_quat)
create_inverse_reachability(pr2, box, table)
ir_database = load_inverse_reachability()
print len(ir_database)
return
link = link_from_name(pr2, LEFT_ARM_LINK)
point, quat = get_link_pose(pr2, link)
print point, quat
p.addUserDebugLine(origin, point, lineColorRGB=(1, 1, 0)) # addUserDebugText
raw_input('Continue?')
current_conf = get_joint_positions(pr2, movable_joints)
#ik_conf = p.calculateInverseKinematics(pr2, link, point)
#ik_conf = p.calculateInverseKinematics(pr2, link, point, quat)
min_limits = [get_joint_limits(pr2, joint)[0] for joint in movable_joints]
max_limits = [get_joint_limits(pr2, joint)[1] for joint in movable_joints]
max_velocities = [get_max_velocity(pr2, joint) for joint in movable_joints] # Range of Jacobian
print min_limits
print max_limits
print max_velocities
ik_conf = p.calculateInverseKinematics(pr2, link, point, quat, lowerLimits=min_limits,
upperLimits=max_limits, jointRanges=max_velocities, restPoses=current_conf)
value_from_joint = dict(zip(movable_joints, ik_conf))
print [value_from_joint[joint] for joint in joints]
#print len(ik_conf), ik_conf
set_joint_positions(pr2, movable_joints, ik_conf)
#print len(movable_joints), get_joint_positions(pr2, movable_joints)
print get_joint_positions(pr2, joints)
raw_input('Finish?')
p.disconnect()
def open_wsg50_gripper(mbp, context, model_index):
set_joint_positions(get_movable_joints(mbp, model_index), context,
get_open_wsg50_positions(mbp, model_index))
def close_wsg50_gripper(mbp, context, model_index): # 0.05
set_joint_positions(get_movable_joints(mbp, model_index), context,
get_close_wsg50_positions(mbp, model_index))