def activateNailgun(nailGunID,
object1ID,
parentRefCOM,
childRefCOM,
childFrameOrn=[0, 0, 0]):
#check to see if they're touching
gunContact = p.getContactPoints(nailGunID, object1ID)
if not len(gunContact):
print('No Contact')
return False
#get rid of the nasty contact points-just use COM of nailgun + some z component to get pos, and just use straight up as orn
#use ray tracing to determine locations of constrain
gunPos, _ = p.getBasePositionAndOrientation(nailGunID)
rayPos = np.array(gunPos) + np.array([0, 0, 0.165])
rayOrn = np.array([0, 0, 1])
rayDist = 0.25
#rayOrn = np.array([0,0,1])
#Check and see if the ray intersects both objects
rayTest = p.rayTest(rayPos, rayPos + rayDist * rayOrn)[0]
hitID = rayTest[0]
hitPos = np.array(rayTest[3])
#Add constrain
constraintID = p.createConstraint(object1ID,
-1,
hitID,
-1,
p.JOINT_FIXED,
hitPos,
parentRefCOM,
childRefCOM,
childFrameOrientation=childFrameOrn)
print("Finished")
print(p.getConstraintInfo(constraintID))
return constraintID
def check_grasp(self):
"""Check a grasp (object in contact?) for picking success."""
suctioned_object = None
if self.contact_constraint is not None:
suctioned_object = p.getConstraintInfo(self.contact_constraint)[2]
return suctioned_object is not None
def get_constraint_max_force(self, constraint_uid):
"""Get the maximal force of the constraint.
Args:
constraint_uid: The constraint unique ID.
Returns:
A 3-dimensional float32 numpy array.
"""
_, _, _, _, _, _, _, _, _, _, max_force, _, _, _, _ = (
pybullet.getConstraintInfo(constraintUniqueId=constraint_uid,
physicsClientId=self.uid))
return np.array(max_force, dtype=np.float32)
def get_constraint_orientation(self, constraint_uid):
"""Get the constraint orientation.
Args:
constraint_uid: The constraint unique ID.
Returns:
An instance of Orientation.
"""
_, _, _, _, _, _, _, _, _, quaternion, _, _, _, _, _ = (
pybullet.getConstraintInfo(constraintUniqueId=constraint_uid,
physicsClientId=self.uid))
return Orientation(quaternion)
def get_constraint_position(self, constraint_uid):
"""Get the constraint position.
Args:
constraint_uid: The constraint unique ID.
Returns:
A 3-dimenstional float32 numpy array.
"""
_, _, _, _, _, _, _, position, _, _, _, _, _, _, _ = (
pybullet.getConstraintInfo(constraintUniqueId=constraint_uid,
physicsClientId=self.uid))
return np.array(position, dtype=np.float32)
def get_constraint_pose(self, constraint_uid):
"""Get the constraint pose.
Args:
constraint_uid: The constraint unique ID.
Returns:
An instance of Pose.
"""
_, _, _, _, _, _, _, position, _, quaternion, _, _, _, _, _ = (
pybullet.getConstraintInfo(constraintUniqueId=constraint_uid,
physicsClientId=self.uid))
return Pose([position, quaternion])
def check_grasp(self):
"""Check a grasp for picking success.
If picking fails, then robot doesn't do the place action. For rigid
items: index 2 in getConstraintInfo returns childBodyUniqueId. For
deformables, check the length of the anchors.
Returns:
bool indicating if contact constraint is not None (gripped a rigid item)
or if there exists at least one grip anchor (gripped a soft body).
"""
# TODO(daniel) I left this code untouched but we never use suctioned_object?
suctioned_object = None
if self.contact_constraint is not None:
suctioned_object = p.getConstraintInfo(self.contact_constraint)[2]
del suctioned_object
pick_deformable = False
if self.def_grip_anchors is not None:
pick_deformable = len(self.def_grip_anchors) > 0 # pylint: disable=g-explicit-length-test
return (self.contact_constraint is not None) or pick_deformable
def detect_contact(self):
"""Detects a contact with a rigid object."""
body, link = self.body, 0
if self.activated and self.contact_constraint is not None:
try:
info = p.getConstraintInfo(self.contact_constraint)
body, link = info[2], info[3]
except: # pylint: disable=bare-except
self.contact_constraint = None
pass
# Get all contact points between the suction and a rigid body.
points = p.getContactPoints(bodyA=body, linkIndexA=link)
# print(points)
# exit()
if self.activated:
points = [point for point in points if point[2] != self.body]
# # We know if len(points) > 0, contact is made with SOME rigid item.
if points:
return True
return False
def get_constraint_info(constraint):
# there are four additional arguments
return ConstraintInfo(*p.getConstraintInfo(constraint)[:11])
if jointName == mimicParentName:
mimicParentID = jointID
print("There are {} mimic child".format(len(mimicChildList)))
# contraints to simulate mimic tag in robotiq_c2.urdf
for i, mimicChildID in enumerate(mimicChildList):
c = p.createConstraint(robotID,
mimicParentID,
robotID,
mimicChildID,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=mimicMul[i], maxForce=10000)
constraintInfo = p.getConstraintInfo(c)
# start simulation
try:
flag = True
gripper_control = p.addUserDebugParameter("gripper", 0, 0.343, 0)
while (flag):
jointPose = p.readUserDebugParameter(gripper_control)
p.setJointMotorControl2(robotID,
mimicParentID,
p.POSITION_CONTROL,
targetPosition=jointPose)
p.stepSimulation()
p.disconnect()
except:
p.disconnect()
def detect_contact(self, def_ids):
"""Detects a contact for either gripping or releasing purposes.
If suction off, detect contact between gripper and objects.
If suction on, detect contact between picked object and other objects.
After checking if contact with a rigid item, then proceed to checking
for contacts with deformable. TODO(daniel) see if we can cache
computation or speed it up in some way, however my profiling using
cProfile did not indicate that this was a bottleneck.
Args:
def_ids: List of IDs of deformables (if any). It may be
computationally intensive to keep querying distances, so if we
have any contact points with rigid objects, we return right away
without checking for contact with deformables.
Returns:
bool indicating contact.
"""
body, link = self.body, 0
if self.activated and self.contact_constraint is not None:
try:
info = p.getConstraintInfo(self.contact_constraint)
body, link = info[2], info[3]
except: # pylint: disable=bare-except
self.contact_constraint = None
pass
# Get all contact points between the suction and a rigid body.
points = p.getContactPoints(bodyA=body, linkIndexA=link)
if self.activated:
points = [point for point in points if point[2] != self.body]
# We know if len(points) > 0, contact is made with SOME rigid item.
if points:
return True
# If there's no deformables, return False, there can't be any contact.
if not def_ids:
return False
# If suction off (not activated) and len(points)==0, check contact w/defs.
if not self.activated:
gripper_position = np.float32(p.getLinkState(self.body, 0)[0])
for id_ in def_ids:
if self.detect_contact_def(gripper_position, id_):
self.def_grip_item = id_
return True
return False
# Now either (a) gripping a def., or (b) gripping a rigid item that is
# NOT touching another rigid item, but _may_ be touching a def.
assert self.activated
if self.init_grip_item is not None:
# If suction on, check if a gripped RIGID item touches a deformable.
# When this happens, the suction 'goes through' its gripped item,
# and hence the distance between it and the object decreases.
object_pose = p.getBasePositionAndOrientation(self.init_grip_item)
gripper_position = np.float32(p.getLinkState(self.body, 0)[0])
d = gripper_position - np.float32(object_pose[0])
distance = np.linalg.norm(d)
fraction = distance / self.init_grip_distance
if fraction < 0.92 or fraction > 2.0:
self.init_grip_distance = None
self.init_grip_item = None
return fraction < 0.92 or fraction > 2.0
elif self.def_grip_item is not None:
# This is for making deformable-deformable contact, but I dont
# think we ever trigger this condition since something else kicks
# in first (e.g., height of gripper, or touching rigid).
# TODO(daniel) confirm.
# TODO(daniel) I was going to use self.def_min_vertex here I think.
return False
# We should always be gripping a rigid or a def., so code shouldn't reach
# here.
return False
def detect_contact(self, def_IDs):
"""
If suction off, detect contact between gripper and objects.
If suction on, detect contact between picked object and other objects.
:def_IDs: a list of IDs of deformable objects. Since it may be
computationally heavy to check, if we have any contact points with
rigid we just return that right away (w/out checking deformables).
"""
body, link = self.body, 0
if self.activated and self.contact_constraint is not None:
try:
info = p.getConstraintInfo(self.contact_constraint)
body, link = info[2], info[3]
except:
self.contact_constraint = None
pass
# Get all contact points between suction and a rigid body.
points = p.getContactPoints(bodyA=body, linkIndexA=link)
if self.activated:
points = [point for point in points if point[2] != self.body]
# Normally we return if len(points) > 0, but now if len == 0, we might be
# missing (a) gripping a deformable or (b) rigid item hitting a deformable.
if len(points) > 0:
return True
# If suction off and len(points)==0, check contact w/deformables. Note:
# it is critical that we set `self.def_grip_item` correctly.
if not self.activated:
gripper_position = np.float32(p.getLinkState(self.body, 0)[0])
for ID in def_IDs:
if self.detect_contact_def(gripper_position, ID):
self.def_grip_item = ID
return True
return False
# Here, we're either (a) gripping a deformable, or (b) gripping a rigid
# item that is NOT touching another rigid item.
assert self.activated
if (self.init_grip_item is not None):
# If suction on, check if a gripped RIGID item touches a deformable.
# For this, tests were applying if the gripper went through the
# rigid item, but perhaps also if the fraction is too high?
# [Haven't seen this in any test scenario, though.]
object_pose = p.getBasePositionAndOrientation(self.init_grip_item)
gripper_position = np.float32(p.getLinkState(self.body, 0)[0])
d = gripper_position - np.float32(object_pose[0])
distance = np.linalg.norm(d)
fraction = distance / self.init_grip_distance
if (fraction <= self.def_frac_lower) or (fraction >=
self.def_frac_upper):
#print(f'[gripping rigid], distance: {distance:0.5f} vs original: '
# f'{self.init_grip_distance:0.5f}, and fraction: {fraction:0.5f}')
# Will release this item, so we don't need these until the next suction.
self.init_grip_distance = None
self.init_grip_item = None
return (fraction <= self.def_frac_lower) or (fraction >=
self.def_frac_upper)
elif (self.def_grip_item is not None):
# Should see if I ever encounter this in practice? With cloth-cover
# and cloth-flat I don't think we trigger this condition. UPDATE:
# ah, upon further tests, that's because the len(points)>0 trigger
# kicks in when we grip a deformable, but where the suction makes
# contact. If that happens I'm fine, reduces complexity.
#
# TODO: actually need to fix, I think we need suction gripper and
# the robot's joint just before that. This will not get activated.
return False
else:
# I don't think this should ever invoke -- we should always be
# gripping a rigid or deformable in this condition.
return False
def get_constraint_info(constraint): # getConstraintState
# TODO: four additional arguments
return ConstraintInfo(*p.getConstraintInfo(constraint, physicsClientId=CLIENT)[:11])
def getConstraintInfo(self, constraintUniqueId):
return p.getConstraintInfo(self.kukaUid, constraintUniqueId)
