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 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 is_fixed_base(body):
return get_mass(body) == STATIC_MASS