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 joint_controller_hold(body, joints, target, **kwargs):
"""
Keeps other joints in place
"""
movable_joints = get_movable_joints(body)
conf = list(get_joint_positions(body, movable_joints))
for joint, value in zip(movable_from_joints(body, joints), target):
conf[joint] = value
return joint_controller(body, movable_joints, conf, **kwargs)
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 joint_controller_hold2(body,
joints,
positions,
velocities=None,
tolerance=1e-2 * np.pi,
position_gain=0.05,
velocity_gain=0.01):
"""
Keeps other joints in place
"""
# TODO: velocity_gain causes the PR2 to oscillate
if velocities is None:
velocities = [0.] * len(positions)
movable_joints = get_movable_joints(body)
target_positions = list(get_joint_positions(body, movable_joints))
target_velocities = [0.] * len(movable_joints)
movable_from_original = {o: m for m, o in enumerate(movable_joints)}
#print(list(positions), list(velocities))
for joint, position, velocity in zip(joints, positions, velocities):
target_positions[movable_from_original[joint]] = position
target_velocities[movable_from_original[joint]] = velocity
# return joint_controller(body, movable_joints, conf)
current_conf = get_joint_positions(body, movable_joints)
#forces = [get_max_force(body, joint) for joint in movable_joints]
while not np.allclose(
current_conf, target_positions, atol=tolerance, rtol=0):
# TODO: only enforce velocity constraints at end
p.setJointMotorControlArray(
body,
movable_joints,
p.POSITION_CONTROL,
targetPositions=target_positions,
#targetVelocities=target_velocities,
positionGains=[position_gain] * len(movable_joints),
#velocityGains=[velocity_gain] * len(movable_joints),
#forces=forces,
physicsClientId=CLIENT)
yield current_conf
current_conf = get_joint_positions(body, movable_joints)
def plan_cartesian_motion(robot,
first_joint,
target_link,
waypoint_poses,
max_iterations=200,
custom_limits={},
get_sub_conf=False,
**kwargs):
"""Compute a joint trajectory for a given sequence of workspace poses. Only joint limit is considered.
Collision checking using `get_collision_fn` is often performed on the path computed by this function.
Parameters
----------
robot : int
robot body index
first_joint : int
the first joint index in the kinematics chain.
target_link : int
end effector link index.
waypoint_poses : a list of Pose
a list of end effector workspace poses in the world coord.
max_iterations : int, optional
[description], by default 200
custom_limits : dict, optional
[description], by default {}
get_sub_conf : bool, optional
return sub-kinematics chain configuration if set to True, by default False
Returns
-------
[type]
[description]
Example
-------
```
ik_joints = joints_from_names(robot_uid, ik_joint_names)
ik_tool_link = link_from_name(robot_uid, tool_link_name)
cart_conf_vals = plan_cartesian_motion(robot_uid, ik_joints[0], ik_tool_link, world_from_ee_poses)
```
"""
from pybullet_planning.interfaces.env_manager.pose_transformation import all_between
from pybullet_planning.interfaces.robots.link import get_link_subtree, prune_fixed_joints
from pybullet_planning.interfaces.kinematics import inverse_kinematics_helper, is_pose_close
# TODO: fix stationary joints
# TODO: pass in set of movable joints and take least common ancestor
# TODO: update with most recent bullet updates
# https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_kinematics.py
# https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_kinematics_husky_kuka.py
# TODO: plan a path without needing to following intermediate waypoints
lower_limits, upper_limits = get_custom_limits(robot,
get_movable_joints(robot),
custom_limits)
selected_links = get_link_subtree(
robot, first_joint) # TODO: child_link_from_joint?
selected_movable_joints = prune_fixed_joints(robot, selected_links)
assert (target_link in selected_links)
selected_target_link = selected_links.index(target_link)
sub_robot = clone_body(robot,
links=selected_links,
visual=False,
collision=False) # TODO: joint limits
sub_movable_joints = get_movable_joints(sub_robot)
#null_space = get_null_space(robot, selected_movable_joints, custom_limits=custom_limits)
null_space = None
solutions = []
for target_pose in waypoint_poses:
for iteration in range(max_iterations):
sub_kinematic_conf = inverse_kinematics_helper(
sub_robot,
selected_target_link,
target_pose,
null_space=null_space)
if sub_kinematic_conf is None:
remove_body(sub_robot)
return None
set_joint_positions(sub_robot, sub_movable_joints,
sub_kinematic_conf)
if is_pose_close(get_link_pose(sub_robot, selected_target_link),
target_pose, **kwargs):
set_joint_positions(robot, selected_movable_joints,
sub_kinematic_conf)
kinematic_conf = get_configuration(robot)
if not all_between(lower_limits, kinematic_conf, upper_limits):
#movable_joints = get_movable_joints(robot)
#print([(get_joint_name(robot, j), l, v, u) for j, l, v, u in
# zip(movable_joints, lower_limits, kinematic_conf, upper_limits) if not (l <= v <= u)])
#print("Limits violated")
#wait_for_user()
remove_body(sub_robot)
return None
#print("IK iterations:", iteration)
if not get_sub_conf:
solutions.append(kinematic_conf)
else:
solutions.append(sub_kinematic_conf)
break
else:
remove_body(sub_robot)
return None
remove_body(sub_robot)
return solutions
def plan_cartesian_motion(robot,
first_joint,
target_link,
waypoint_poses,
max_iterations=200,
custom_limits={},
**kwargs):
from pybullet_planning.interfaces.env_manager.pose_transformation import all_between
from pybullet_planning.interfaces.robots.link import get_link_subtree, prune_fixed_joints
from pybullet_planning.interfaces.kinematics import inverse_kinematics_helper, is_pose_close
# TODO: fix stationary joints
# TODO: pass in set of movable joints and take least common ancestor
# TODO: update with most recent bullet updates
# https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_kinematics.py
# https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_kinematics_husky_kuka.py
# TODO: plan a path without needing to following intermediate waypoints
lower_limits, upper_limits = get_custom_limits(robot,
get_movable_joints(robot),
custom_limits)
selected_links = get_link_subtree(
robot, first_joint) # TODO: child_link_from_joint?
selected_movable_joints = prune_fixed_joints(robot, selected_links)
assert (target_link in selected_links)
selected_target_link = selected_links.index(target_link)
sub_robot = clone_body(robot,
links=selected_links,
visual=False,
collision=False) # TODO: joint limits
sub_movable_joints = get_movable_joints(sub_robot)
#null_space = get_null_space(robot, selected_movable_joints, custom_limits=custom_limits)
null_space = None
solutions = []
for target_pose in waypoint_poses:
for iteration in range(max_iterations):
sub_kinematic_conf = inverse_kinematics_helper(
sub_robot,
selected_target_link,
target_pose,
null_space=null_space)
if sub_kinematic_conf is None:
remove_body(sub_robot)
return None
set_joint_positions(sub_robot, sub_movable_joints,
sub_kinematic_conf)
if is_pose_close(get_link_pose(sub_robot, selected_target_link),
target_pose, **kwargs):
set_joint_positions(robot, selected_movable_joints,
sub_kinematic_conf)
kinematic_conf = get_configuration(robot)
if not all_between(lower_limits, kinematic_conf, upper_limits):
#movable_joints = get_movable_joints(robot)
#print([(get_joint_name(robot, j), l, v, u) for j, l, v, u in
# zip(movable_joints, lower_limits, kinematic_conf, upper_limits) if not (l <= v <= u)])
#print("Limits violated")
#wait_for_user()
remove_body(sub_robot)
return None
#print("IK iterations:", iteration)
solutions.append(kinematic_conf)
break
else:
remove_body(sub_robot)
return None
remove_body(sub_robot)
return solutions