def compute_base_path_to_cluster_init(i_cluster, qcurrent=None):
if qcurrent is None:
import estimation
qcurrent = estimation.get_current_config(robot, graph, q0[:])
if not robot.configIsValid(qcurrent): print("qcurrent invalid")
# Tuck arm.
res, qtuck, err = graph.generateTargetConfig('end_arm', qcurrent, qcurrent)
if not res: print("failed to tuck arm")
if not robot.configIsValid(qtuck): print("qtuck invalid")
tuckpath = armPlanner.computePath(qcurrent, qtuck, resetRoadmap=True)
# Move mobile base.
qhome = clusters[i_cluster][0][1][:4] + qtuck[4:]
res, qhome, err = graph.generateTargetConfig('move_base', qhome, qhome)
if not res: print("failed to move base")
basepath = basePlanner.computePath(
qtuck, qhome, resetRoadmap=not basePlannerUsePrecomputedRoadmap)
# Move the head.
# Compute config where the robot looks at the part
constraints = wd(armPlanner.cconstraints.getConfigProjector().copy())
constraints.add(look_at_part, 0)
constraints.setRightHandSideFromConfig(qhome)
res, qend = constraints.apply(qhome)
if not res: print("failed to look at the part. It may not be visible.")
headpath = armPlanner.computePath(qhome, qend, resetRoadmap=True)
tuckpath.concatenate(basepath)
tuckpath.concatenate(headpath)
return tuckpath
def recompute_clusters(handles=None, qcurrent=None):
if qcurrent is None:
import estimation
qcurrent = estimation.get_current_config(robot, graph, q0)
try:
qcurrent = estimation.get_cylinder_pose(robot,
qcurrent,
timeout=0.5)
except RuntimeError:
pass
if handles is None:
handles = part_handles
setRobotJointBounds("grasp-generation")
clusters = clusters_comp.find_clusters(handles,
qcurrent,
N_find_first=40,
N_find_others=40)
setRobotJointBounds("planning")
return clusters
def compute_base_path_to_cluster_init(i_cluster, qcurrent=None):
if qcurrent is None:
import estimation
qcurrent = estimation.get_current_config(robot, graph, q0)
try:
qcurrent = estimation.get_cylinder_pose(robot,
qcurrent,
timeout=0.5)
except RuntimeError:
pass
setRobotJointBounds("default")
valid, msg = robot.isConfigValid(qcurrent)
outOfCollisionPath = None
if not valid:
if msg == 'Collision between object tiago/torso_fixed_column_link_0 and tiago/hand_safety_box_0' \
or msg == 'Collision between object tiago/torso_fixed_column_link_0 and driller/base_link_0' \
or msg == 'Collision between object tiago/base_link_0 and driller/tag_support_link_top_0' \
or msg == 'Collision between object tiago/torso_fixed_column_link_0 and driller/tag_support_link_back_0':
qcurrent2 = qcurrent[:]
qcurrent2[robot.rankInConfiguration['tiago/arm_6_joint']] = 1.
res, qcurrent2, err = graph.applyNodeConstraints(
"tiago/gripper grasps driller/handle", qcurrent2)
if not res:
print("could not get out of collision")
else:
csm = armPlanner.cproblem.getSteeringMethod()
outOfCollisionPath = csm.call(qcurrent, qcurrent2)
qcurrent = qcurrent2
else:
print("qcurrent is in collision")
# Tuck arm.
res, qtuck, err = graph.generateTargetConfig('end_arm', qcurrent, qcurrent)
if not res: print("failed to tuck arm")
if not robot.configIsValid(qtuck): print("qtuck invalid")
tuckpath = armPlanner.computePath(qcurrent, qtuck, resetRoadmap=True)
#tuckpath = armPlanner.timeParameterization(tuckpath)
setRobotJointBounds("planning")
# Move mobile base.
qhome = clusters[i_cluster][0][1][:4] + qtuck[4:]
res, qhome, err = graph.generateTargetConfig('move_base', qhome, qhome)
if not res: print("failed to move base")
basepath = basePlanner.computePath(
qtuck, qhome, resetRoadmap=not basePlannerUsePrecomputedRoadmap)
# Move the head.
# Compute config where the robot looks at the part
constraints = wd(armPlanner.cconstraints.getConfigProjector().copy())
constraints.add(look_at_part, 0)
constraints.setRightHandSideFromConfig(qhome)
res, qend = constraints.apply(qhome)
if not res: print("failed to look at the part. It may not be visible.")
headpath = armPlanner.computePath(qhome, qend, resetRoadmap=True)
#headpath = armPlanner.timeParameterization(headpath)
if outOfCollisionPath is not None:
path = outOfCollisionPath.asVector()
path.concatenate(tuckpath)
else:
path = tuckpath
path.concatenate(basepath)
path.concatenate(headpath)
return path