def compute_joint_weights(robot, num=100):
import time
from pybullet_planning.interfaces.planner_interface.joint_motion_planning import get_sample_fn
from pybullet_planning.interfaces.robots.link import get_links
from pybullet_planning.interfaces.robots.dynamics import get_mass
# http://openrave.org/docs/0.6.6/_modules/openravepy/databases/linkstatistics/#LinkStatisticsModel
# TODO: use velocities instead
start_time = time.time()
joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, joints)
weighted_jacobian = np.zeros(len(joints))
links = list(get_links(robot))
# links = {l for j in joints for l in get_link_descendants(self.robot, j)}
masses = [get_mass(robot, link) for link in links] # Volume, AABB volume
total_mass = sum(masses)
for _ in range(num):
conf = sample_fn()
for mass, link in zip(masses, links):
translate, rotate = compute_jacobian(robot, link, conf)
weighted_jacobian += np.array(
[mass * np.linalg.norm(vec) for vec in translate]) / total_mass
weighted_jacobian /= num
print(list(weighted_jacobian))
print(time.time() - start_time)
return weighted_jacobian
def dump_body(body, fixed=False):
print('Body id: {} | Name: {} | Rigid: {} | Fixed: {}'.format(
body, get_body_name(body), is_rigid_body(body), is_fixed_base(body)))
for joint in get_joints(body):
if fixed or is_movable(body, joint):
print(
'Joint id: {} | Name: {} | Type: {} | Circular: {} | Limits: {}'
.format(joint, get_joint_name(body, joint),
JOINT_TYPES[get_joint_type(body, joint)],
is_circular(body,
joint), get_joint_limits(body, joint)))
link = NULL_ID
print('Link id: {} | Name: {} | Mass: {} | Collision: {} | Visual: {}'.
format(link, get_base_name(body), get_mass(body),
len(get_collision_data(body, link)),
NULL_ID)) # len(get_visual_data(body, link))))
for link in get_links(body):
joint = parent_joint_from_link(link)
joint_name = JOINT_TYPES[get_joint_type(body, joint)] if is_fixed(
body, joint) else get_joint_name(body, joint)
print(
'Link id: {} | Name: {} | Joint: {} | Parent: {} | Mass: {} | Collision: {} | Visual: {}'
.format(link, get_link_name(body, link), joint_name,
get_link_name(body, get_link_parent(body, link)),
get_mass(body, link), len(get_collision_data(body, link)),
NULL_ID)) # len(get_visual_data(body, link))))
def get_self_link_pairs(body,
joints,
disabled_collisions=set(),
only_moving=True):
from pybullet_planning.interfaces.robots.link import get_links, are_links_adjacent
moving_links = get_moving_links(body, joints)
fixed_links = list(set(get_links(body)) - set(moving_links))
check_link_pairs = list(product(moving_links, fixed_links))
if only_moving:
check_link_pairs.extend(get_moving_pairs(body, joints))
else:
check_link_pairs.extend(combinations(moving_links, 2))
check_link_pairs = list(
filter(lambda pair: not are_links_adjacent(body, *pair),
check_link_pairs))
check_link_pairs = list(
filter(
lambda pair: (pair not in disabled_collisions) and
(pair[::-1] not in disabled_collisions), check_link_pairs))
return check_link_pairs
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 draw_collision_diagnosis(pb_closest_pt_output, viz_last_duration=-1, line_color=YELLOW, \
focus_camera=True, camera_ray=np.array([0.1, 0, 0.05])):
"""[summary]
Parameters
----------
pb_closest_pt_output : [type]
[description]
viz_last_duration : int, optional
[description], by default -1
"""
from pybullet_planning.interfaces.env_manager.simulation import has_gui
from pybullet_planning.interfaces.env_manager.user_io import HideOutput
from pybullet_planning.interfaces.env_manager.user_io import wait_for_user, wait_for_duration
from pybullet_planning.interfaces.robots.link import get_link_name, get_links
from pybullet_planning.interfaces.robots.body import set_color, remove_body, clone_body, get_name
if not has_gui() and pb_closest_pt_output:
return
# if paint_all_others:
# set_all_bodies_color()
# for b in obstacles:
# set_color(b, (0,0,1,0.3))
for u_cr in pb_closest_pt_output:
handles = []
b1 = get_body_from_pb_id(u_cr[1])
b2 = get_body_from_pb_id(u_cr[2])
l1 = u_cr[3]
l2 = u_cr[4]
b1_name = get_name(b1)
b2_name = get_name(b2)
l1_name = get_link_name(b1, l1)
l2_name = get_link_name(b2, l2)
print('*' * 10)
print(
'pairwise link collision: (Body #{0}, Link #{1}) - (Body #{2} Link #{3})'
.format(b1_name, l1_name, b2_name, l2_name))
clone1_fail = False
clone2_fail = False
try:
with HideOutput():
cloned_body1 = clone_body(
b1,
links=[l1] if get_links(b1) else None,
collision=True,
visual=False)
except:
print('cloning (body #{}, link #{}) fails.'.format(
b1_name, l1_name))
clone1_fail = True
cloned_body1 = b1
try:
with HideOutput():
cloned_body2 = clone_body(
b2,
links=[l2] if get_links(b2) else None,
collision=True,
visual=False)
except:
print('cloning (body #{}, link #{}) fails.'.format(
b2_name, l2_name))
clone2_fail = True
cloned_body2 = b2
set_color(cloned_body1, apply_alpha(RED, 0.2))
set_color(cloned_body2, apply_alpha(GREEN, 0.2))
handles.append(add_body_name(b1))
handles.append(add_body_name(b2))
handles.append(draw_link_name(b1, l1))
handles.append(draw_link_name(b2, l2))
handles.append(add_line(u_cr[5], u_cr[6], color=line_color, width=5))
print('Penetration depth: {:.2E}'.format(get_distance(
u_cr[5], u_cr[6])))
if focus_camera:
camera_base_pt = u_cr[5]
camera_pt = np.array(camera_base_pt) + camera_ray
set_camera_pose(tuple(camera_pt), camera_base_pt)
if viz_last_duration < 0:
wait_for_user()
else:
wait_for_duration(viz_last_duration)
# restore lines and colors
for h in handles:
remove_debug(h)
if not clone1_fail:
remove_body(cloned_body1)
else:
set_color(b1, apply_alpha(WHITE, 0.5))
if not clone2_fail:
remove_body(cloned_body2)
else:
set_color(b2, apply_alpha(WHITE, 0.5))