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 to_data(self):
from pybullet_planning.interfaces.robots.body import get_body_name, get_name
from pybullet_planning.interfaces.robots.link import get_link_name
data = {}
data['parent_name'] = get_body_name(self.parent)
data['parent_link_name'] = get_link_name(self.parent, self.parent_link)
data['grasp_pose'] = self.grasp_pose
child_name = get_body_name(self.child)
data['child_name'] = get_name(
self.child) if child_name == '' else child_name
return data
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))
def draw_link_name(body, link=BASE_LINK):
from pybullet_planning.interfaces.robots.link import get_link_name
return add_text(get_link_name(body, link),
position=(0, 0.2, 0),
parent=body,
parent_link=link)