def compute_jacobian(robot, link, positions=None):
joints = get_movable_joints(robot)
if positions is None:
positions = get_joint_positions(robot, joints)
assert len(joints) == len(positions)
velocities = [0.0] * len(positions)
accelerations = [0.0] * len(positions)
translate, rotate = p.calculateJacobian(robot,
link,
unit_point(),
positions,
velocities,
accelerations,
physicsClientId=CLIENT)
#movable_from_joints(robot, joints)
return list(zip(*translate)), list(zip(*rotate)) # len(joints) x 3
def select_solution(body,
joints,
solutions,
nearby_conf=True,
random=False,
**kwargs):
"""select one joint configuration given a list of them
Parameters
----------
body : pybullet body
This can be any pybullet body, including the robot
joints : a list of pybullet joint
solutions : a list of float-lists
joint values
nearby_conf : bool
return the joint conf that is closest to the current conf in pybullet env,
defaults to True
random : bool
randomly chose one
**kwargs : optional
additional arguments for the cost function when ranking
Returns
-------
a list of lists or one list
the joint configuration(s)
"""
if not solutions:
return None
if random and not nearby_conf:
return random.choice(solutions)
if nearby_conf:
nearby_conf = get_joint_positions(body, joints)
# TODO: sort by distance before collision checking
# TODO: search over neighborhood of sampled joints when nearby_conf != None
return min(solutions,
key=lambda conf: get_distance(nearby_conf, conf, **kwargs))
else:
return random.choice(solutions)
def plan_nonholonomic_motion(body,
joints,
end_conf,
obstacles=[],
attachments=[],
self_collisions=True,
disabled_collisions=set(),
weights=None,
resolutions=None,
reversible=True,
max_distance=MAX_DISTANCE,
custom_limits={},
**kwargs):
from pybullet_planning.interfaces.robots.joint import get_joint_positions
assert len(joints) == len(end_conf)
sample_fn = get_sample_fn(body, joints, custom_limits=custom_limits)
distance_fn = get_nonholonomic_distance_fn(body,
joints,
weights=weights,
reversible=reversible)
extend_fn = get_nonholonomic_extend_fn(body,
joints,
resolutions=resolutions,
reversible=reversible)
collision_fn = get_collision_fn(body,
joints,
obstacles,
attachments,
self_collisions,
disabled_collisions,
custom_limits=custom_limits,
max_distance=max_distance)
start_conf = get_joint_positions(body, joints)
if not check_initial_end(start_conf, end_conf, collision_fn):
return None
return birrt(start_conf, end_conf, distance_fn, sample_fn, extend_fn,
collision_fn, **kwargs)
def plan_joint_motion(body,
joints,
end_conf,
obstacles=[],
attachments=[],
self_collisions=True,
disabled_collisions=set(),
extra_disabled_collisions=set(),
weights=None,
resolutions=None,
max_distance=MAX_DISTANCE,
custom_limits={},
diagnosis=False,
**kwargs):
"""call birrt to plan a joint trajectory from the robot's **current** conf to ``end_conf``.
"""
assert len(joints) == len(end_conf)
sample_fn = get_sample_fn(body, joints, custom_limits=custom_limits)
distance_fn = get_distance_fn(body, joints, weights=weights)
extend_fn = get_extend_fn(body, joints, resolutions=resolutions)
collision_fn = get_collision_fn(
body,
joints,
obstacles=obstacles,
attachments=attachments,
self_collisions=self_collisions,
disabled_collisions=disabled_collisions,
extra_disabled_collisions=extra_disabled_collisions,
custom_limits=custom_limits,
max_distance=max_distance)
start_conf = get_joint_positions(body, joints)
if not check_initial_end(
start_conf, end_conf, collision_fn, diagnosis=diagnosis):
return None
return birrt(start_conf, end_conf, distance_fn, sample_fn, extend_fn,
collision_fn, **kwargs)
def plan_waypoints_joint_motion(body,
joints,
waypoints,
start_conf=None,
obstacles=[],
attachments=[],
self_collisions=True,
disabled_collisions=set(),
resolutions=None,
custom_limits={},
max_distance=MAX_DISTANCE):
extend_fn = get_extend_fn(body, joints, resolutions=resolutions)
collision_fn = get_collision_fn(body,
joints,
obstacles,
attachments,
self_collisions,
disabled_collisions,
custom_limits=custom_limits,
max_distance=max_distance)
if start_conf is None:
start_conf = get_joint_positions(body, joints)
else:
assert len(start_conf) == len(joints)
for i, waypoint in enumerate([start_conf] + list(waypoints)):
if collision_fn(waypoint):
#print("Warning: waypoint configuration {}/{} is in collision".format(i, len(waypoints)))
return None
path = [start_conf]
for waypoint in waypoints:
assert len(joints) == len(waypoint)
for q in extend_fn(path[-1], waypoint):
if collision_fn(q):
return None
path.append(q)
return path
def get_ik_tool_link_pose(fk_fn, robot, ik_joints, base_link, \
joint_values=None, use_current=False):
"""Use the given forward_kinematics function to compute ik_tool_link pose
based on current joint configurations in pybullet.
Note: The resulting FK pose is relative to the world frame, not the robot's base frame.
Parameters
----------
fk_fn : function handle
fk(a 6-list) : point, rot_matrix
robot : pybullet robot
ik_joint : list of int
a list of pybullet joint index that is registered for IK/FK
can be obtained by e.g.: ``ik_joints = joints_from_names(robot, ik_joint_names)``
base_link : int
robot base link index, can be obtained from ``link_from_name(robot, base_link_name)``
joint_values : list of float
robot joint values for FK computation, value correponds to ik_joint_names, default to None
use_current: bool
if true, use the current configuration in pybullet env for FK, default
to False
Returns
-------
pybullet Pose
Pose = (point, quat) = ([x,y,z], [4-list])
"""
if use_current:
conf = get_joint_positions(robot, ik_joints)
else:
assert joint_values
conf = joint_values
base_from_tool = compute_forward_kinematics(fk_fn, conf)
world_from_base = get_link_pose(robot, base_link)
return multiply(world_from_base, base_from_tool)
def plan_cartesian_motion_lg(robot,
joints,
waypoint_poses,
sample_ik_fn=None,
collision_fn=None,
sample_ee_fn=None,
max_sample_ee_iter=MAX_SAMPLE_ITER,
custom_vel_limits={},
ee_vel=None,
jump_threshold={},
**kwargs):
"""ladder graph cartesian planning, better leveraging ikfast for sample_ik_fn
Parameters
----------
robot : [type]
[description]
joints : [type]
[description]
waypoint_poses : [type]
[description]
sample_ik_fn : [type], optional
[description], by default None
collision_fn : [type], optional
[description], by default None
ee_sample_fn : [type], optional
please please please remember to put an end to the sampling loop!
jump_threshold : [type], optional
Returns
-------
[type]
[description]
"""
assert sample_ik_fn is not None, 'Sample fn must be specified!'
# TODO sanity check samplers
ik_sols = [[] for _ in range(len(waypoint_poses))]
for i, task_pose in enumerate(waypoint_poses):
candidate_poses = [task_pose]
if sample_ee_fn is not None:
# extra dof release, copy to reuse generator
current_ee_fn = copy(sample_ee_fn)
cnt = 0
for p in current_ee_fn(task_pose):
if cnt > max_sample_ee_iter:
warnings.warn('EE dof release generator is called over {} times, likely that you forget to put an exit in the generator. ' + \
'We stop generating here for you.')
break
candidate_poses.append(p)
cnt += 1
for ee_pose in candidate_poses:
conf_list = sample_ik_fn(ee_pose)
if collision_fn is not None:
conf_list = [
conf for conf in conf_list
if conf and not collision_fn(conf, **kwargs)
]
ik_sols[i].extend(conf_list)
# assemble the ladder graph
dof = len(joints)
graph = LadderGraph(dof)
graph.resize(len(ik_sols))
# assign rung data
for pt_id, ik_confs_pt in enumerate(ik_sols):
graph.assign_rung(pt_id, ik_confs_pt)
joint_jump_threshold = []
for joint in joints:
if joint in custom_vel_limits:
joint_jump_threshold.append(custom_vel_limits[joint])
else:
joint_jump_threshold.append(INF)
# build edges within current pose family
for i in range(graph.get_rungs_size() - 1):
st_rung_id = i
end_rung_id = i + 1
jt1_list = graph.get_data(st_rung_id)
jt2_list = graph.get_data(end_rung_id)
st_size = graph.get_rung_vert_size(st_rung_id)
end_size = graph.get_rung_vert_size(end_rung_id)
# if st_size == 0 or end_size == 0:
# print(ik_sols)
assert st_size > 0, 'Ladder graph not valid: rung {}/{} is a zero size rung'.format(
st_rung_id, graph.get_rungs_size())
assert end_size > 0, 'Ladder graph not valid: rung {}/{} is a zero size rung'.format(
end_rung_id, graph.get_rungs_size())
# TODO: preference_cost using pose deviation
# fully-connected ladder graph
edge_builder = EdgeBuilder(st_size,
end_size,
dof,
jump_threshold=joint_jump_threshold,
preference_cost=1.0)
for k in range(st_size):
st_jt_id = k * dof
for j in range(end_size):
end_jt_id = j * dof
edge_builder.consider(jt1_list[st_jt_id:st_jt_id + dof],
jt2_list[end_jt_id:end_jt_id + dof], j)
edge_builder.next(k)
# print(edge_builder.max_dtheta_)
# print(edge_builder.edge_scratch_)
# print(edge_builder.result)
# TODO: more report information here
if not edge_builder.has_edges:
print(
'Ladder graph: no edge built between {}-{} | joint threshold: {}, max delta jt: {}'
.format(st_rung_id, end_rung_id, joint_jump_threshold,
edge_builder.max_dtheta_))
return None, None
edges = edge_builder.result
graph.assign_edges(i, edges)
# * use current conf in the env as start_conf
start_conf = get_joint_positions(robot, joints)
st_graph = LadderGraph(graph.dof)
st_graph.resize(1)
st_graph.assign_rung(0, [start_conf])
# TODO: upper_tim here
unified_graph = append_ladder_graph(st_graph,
graph,
jump_threshold=joint_jump_threshold)
if unified_graph is None:
return None, None
# perform DAG search
dag_search = DAGSearch(unified_graph)
min_cost = dag_search.run()
# list of confs
path = dag_search.shortest_path()
# delete the start conf
del path[0]
assert len(path) == len(waypoint_poses)
if len(path) == 0:
return None, None
return path, min_cost
def clone_body(body, links=None, collision=True, visual=True, client=None):
from pybullet_planning.utils import get_client
# TODO: names are not retained
# TODO: error with createMultiBody link poses on PR2
# localVisualFrame_position: position of local visual frame, relative to link/joint frame
# localVisualFrame orientation: orientation of local visual frame relative to link/joint frame
# parentFramePos: joint position in parent frame
# parentFrameOrn: joint orientation in parent frame
client = get_client(client) # client is the new client for the body
if links is None:
links = get_links(body)
#movable_joints = [joint for joint in links if is_movable(body, joint)]
new_from_original = {}
base_link = get_link_parent(body, links[0]) if links else BASE_LINK
new_from_original[base_link] = -1
masses = []
collision_shapes = []
visual_shapes = []
positions = [] # list of local link positions, with respect to parent
orientations = [] # list of local link orientations, w.r.t. parent
inertial_positions = [] # list of local inertial frame pos. in link frame
inertial_orientations = [] # list of local inertial frame orn. in link frame
parent_indices = []
joint_types = []
joint_axes = []
for i, link in enumerate(links):
new_from_original[link] = i
joint_info = get_joint_info(body, link)
dynamics_info = get_dynamics_info(body, link)
masses.append(dynamics_info.mass)
collision_shapes.append(clone_collision_shape(body, link, client) if collision else -1)
visual_shapes.append(clone_visual_shape(body, link, client) if visual else -1)
point, quat = get_local_link_pose(body, link)
positions.append(point)
orientations.append(quat)
inertial_positions.append(dynamics_info.local_inertial_pos)
inertial_orientations.append(dynamics_info.local_inertial_orn)
parent_indices.append(new_from_original[joint_info.parentIndex] + 1) # TODO: need the increment to work
joint_types.append(joint_info.jointType)
joint_axes.append(joint_info.jointAxis)
# https://github.com/bulletphysics/bullet3/blob/9c9ac6cba8118544808889664326fd6f06d9eeba/examples/pybullet/gym/pybullet_utils/urdfEditor.py#L169
base_dynamics_info = get_dynamics_info(body, base_link)
base_point, base_quat = get_link_pose(body, base_link)
new_body = p.createMultiBody(baseMass=base_dynamics_info.mass,
baseCollisionShapeIndex=clone_collision_shape(body, base_link, client) if collision else -1,
baseVisualShapeIndex=clone_visual_shape(body, base_link, client) if visual else -1,
basePosition=base_point,
baseOrientation=base_quat,
baseInertialFramePosition=base_dynamics_info.local_inertial_pos,
baseInertialFrameOrientation=base_dynamics_info.local_inertial_orn,
linkMasses=masses,
linkCollisionShapeIndices=collision_shapes,
linkVisualShapeIndices=visual_shapes,
linkPositions=positions,
linkOrientations=orientations,
linkInertialFramePositions=inertial_positions,
linkInertialFrameOrientations=inertial_orientations,
linkParentIndices=parent_indices,
linkJointTypes=joint_types,
linkJointAxis=joint_axes,
physicsClientId=client)
#set_configuration(new_body, get_joint_positions(body, movable_joints)) # Need to use correct client
for joint, value in zip(range(len(links)), get_joint_positions(body, links)):
# TODO: check if movable?
p.resetJointState(new_body, joint, value, targetVelocity=0, physicsClientId=client)
return new_body
def plan_cartesian_motion_lg(robot,
joints,
waypoint_poses,
sample_ik_fn=None,
collision_fn=None,
sample_ee_fn=None,
max_sample_ee_iter=MAX_SAMPLE_ITER,
custom_vel_limits={},
ee_vel=None,
**kwargs):
"""ladder graph cartesian planning, better leveraging ikfast for sample_ik_fn
Parameters
----------
robot : [type]
[description]
joints : [type]
[description]
waypoint_poses : [type]
[description]
sample_ik_fn : [type], optional
[description], by default None
collision_fn : [type], optional
[description], by default None
ee_sample_fn : [type], optional
please please please remember to put an end to the sampling loop!
Returns
-------
[type]
[description]
"""
assert sample_ik_fn is not None, 'Sample fn must be specified!'
# TODO sanity check samplers
ik_sols = [[] for _ in range(len(waypoint_poses))]
upper_times = np.ones(len(waypoint_poses)) * np.inf
for i, task_pose in enumerate(waypoint_poses):
candidate_poses = [task_pose]
if sample_ee_fn is not None:
# * extra dof release, copy to reuse generator
# yaw_sample_size = 20
# yaw_gen = np.linspace(0.0, 2*np.pi, num=yaw_sample_size)
# from pybullet_planning import multiply, Pose, Euler
# for yaw in yaw_gen:
# new_p = multiply(task_pose, Pose(euler=Euler(yaw=yaw)))
# candidate_poses.append(new_p)
current_ee_fn = copy(sample_ee_fn)
cnt = 0
for p in current_ee_fn(task_pose):
if cnt > max_sample_ee_iter:
warnings.warn('EE dof release generator is called over {} times, likely that you forget to put an exit in the generator. ' + \
'We stop generating here for you.')
break
candidate_poses.append(p)
cnt += 1
for ee_pose in candidate_poses:
conf_list = sample_ik_fn(ee_pose)
if collision_fn is None:
conf_list = [
conf for conf in conf_list
if conf and not collision_fn(conf, **kwargs)
]
ik_sols[i].extend(conf_list)
if ee_vel is not None:
assert ee_vel > 0
upper_times[i] = get_distance(task_pose[0], waypoint_poses[
i - 1][0]) / ee_vel if i > 0 else np.inf
if abs(upper_times[i]) < EPS:
upper_times[i] = np.inf
else:
upper_times[i] = np.inf
# upper_times[i] = 0.5
# assemble the ladder graph
dof = len(joints)
graph = LadderGraph(dof)
graph.resize(len(ik_sols))
# assign rung data
for pt_id, ik_confs_pt in enumerate(ik_sols):
graph.assign_rung(pt_id, ik_confs_pt)
joint_vel_limits = get_custom_max_velocity(robot, joints,
custom_vel_limits)
print('joint vel limit: ', joint_vel_limits)
# build edges within current pose family
for i in range(graph.get_rungs_size() - 1):
st_rung_id = i
end_rung_id = i + 1
jt1_list = graph.get_data(st_rung_id)
jt2_list = graph.get_data(end_rung_id)
st_size = graph.get_rung_vert_size(st_rung_id)
end_size = graph.get_rung_vert_size(end_rung_id)
# if st_size == 0 or end_size == 0:
# print(ik_sols)
assert st_size > 0, 'Ladder graph not valid: rung {}/{} is a zero size rung'.format(
st_rung_id, graph.get_rungs_size())
assert end_size > 0, 'Ladder graph not valid: rung {}/{} is a zero size rung'.format(
end_rung_id, graph.get_rungs_size())
# TODO: preference_cost
# fully-connected ladder graph
edge_builder = EdgeBuilder(st_size, end_size, dof, upper_tm=upper_times[i], \
joint_vel_limits=joint_vel_limits, preference_cost=1.0)
for k in range(st_size):
st_jt_id = k * dof
for j in range(end_size):
end_jt_id = j * dof
edge_builder.consider(jt1_list[st_jt_id:st_jt_id + dof],
jt2_list[end_jt_id:end_jt_id + dof], j)
edge_builder.next(k)
edges = edge_builder.result
# TODO: more report information here
assert edge_builder.has_edges, 'no edge built between {}-{}'.format(
st_rung_id, end_rung_id)
# if not edge_builder.has_edges:
# print('no edge built between {}-{}'.format(st_id, end_id))
# return None
graph.assign_edges(i, edges)
# * use current conf in the env as start_conf
start_conf = get_joint_positions(robot, joints)
st_graph = LadderGraph(graph.dof)
st_graph.resize(1)
st_graph.assign_rung(0, [start_conf])
# TODO: upper_tim here
unified_graph = append_ladder_graph(st_graph,
graph,
upper_tm=0.5,
joint_vel_limits=joint_vel_limits)
if unified_graph is None:
return None
# perform DAG search
dag_search = DAGSearch(unified_graph)
min_cost = dag_search.run()
# list of confs
path = dag_search.shortest_path()
# delete the start conf
del path[0]
assert len(path) == len(waypoint_poses)
if len(path) == 0:
return None
return path, min_cost
