def stateValidate(robot_state,
group_name,
constraints=None,
print_depth=False):
sv_srv = rospy.ServiceProxy(DEFAULT_SV_SERVICE, GetStateValidity)
#rospy.loginfo("Collision Checking by using check_state_validity service...")
#rospy.wait_for_service(STATE_VALID)
gsvr = GetStateValidityRequest()
gsvr.robot_state = robot_state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = sv_srv.call(gsvr)
if (not result.valid):
contact_depths = []
for i in xrange(len(result.contacts)):
contact_depths.append(result.contacts[i].depth)
max_depth = max(contact_depths)
if max_depth < 0.0001:
return True
else:
return False
return result.valid
def checkCollision(self, trajectory, num_points=20, limb='left'):
if limb == 'left':
group = self._left_limb
else:
group = self._right_limb
if num_points > 0:
full_trajectory = []
for waypoint_idx in xrange(len(trajectory) - 1):
full_trajectory.append(waypoint)
for idx in xrange(num_points):
interpolated_points = self.interpolate(
trajectory[waypoint_idx], trajectory[waypoint_idx + 1],
num_points)
full_trajectory.extend(interpolated_points)
full_trajectory.append(trajectory[-1])
else:
full_trajectory = trajectory
for traj_point in full_trajectory:
rs = RobotState()
rs.joint_state.name = group.joint_names()
position_list = []
for joint in group.joint_names():
position_list.append(traj_point[joint])
rs.joint_state.position = position_list
gsvr = GetStateValidityRequest()
gsvr.robot_state = rs
gsvr.group_name = limb + '_arm'
result = self.collision_proxy.call(gsvr)
if not result.valid: # if one point is not valid, the trajectory is not valid!! :-(
print("COLLISION FOUND IN TRAJECTORY >:(")
return False
print("TRAJECTORY IS COLLISION-FREE :D")
return True
def js_cb(self, a):
rospy.loginfo('Received array')
js = JointState()
js.name = ['head_pan', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
jList = a.data
jMatrix = np.reshape(jList,(jList.shape[0]/15,15))
i = 0
for pos in jMatrix:
rospy.loginfo(pos)
js.position = pos
gsvr = GetStateValidityRequest()
rs = RobotState()
rs.joint_state = js
gsvr.robot_state = rs
gsvr.group_name = "both_arms"
resp = self.coll_client(gsvr)
if (resp.valid == False):
rospy.loginfo('false')
rospy.loginfo(i)
self.js_pub.publish(0)
return
i = i + 1
self.js_pub.publish(1)
rospy.loginfo('true')
return
def getStateValidity(self,
robot_state,
group_name='both_arms_torso',
constraints=None,
print_depth=False):
"""Given a RobotState and a group name and an optional Constraints
return the validity of the State"""
gsvr = GetStateValidityRequest()
gsvr.robot_state = robot_state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = self.sv_srv.call(gsvr)
# Uncomment below to "pad" obstacles by only returning inCollision=True when
# max penetration depth is above some threshold
# invalid_depth = 0.0001
# if (not result.valid):
# contact_depths = []
# for i in range(len(result.contacts)):
# contact_depths.append(result.contacts[i].depth)
# max_depth = max(contact_depths)
# if max_depth < invalid_depth:
# return True
return result.valid #, max_depth, min_depth
def collisionCallback(self, feedback):
'''
This callback runs on every feedback message received
'''
validity_msg = GetStateValidityRequest(
) # Build message to verify collision
validity_msg.group_name = PLANNING_GROUP
if self.next_pose and (not self.collision):
self.odometry_me.acquire()
x = self.odometry.position.x
y = self.odometry.position.y
z = self.odometry.position.z
# Distance between the robot and the next position
dist = sqrt((self.next_pose.position.x - x)**2 +
(self.next_pose.position.y - y)**2 +
(self.next_pose.position.z - z)**2)
# Pose to verify collision
pose = Transform()
pose.rotation.x = self.odometry.orientation.x
pose.rotation.y = self.odometry.orientation.y
pose.rotation.z = self.odometry.orientation.z
pose.rotation.w = self.odometry.orientation.w
self.odometry_me.release()
#Verify possible collisions on diferent points between the robot and the goal point
# rospy.logerr("\n\n\nCOLLISION CALLBACK: ")
# rospy.logerr(dist)
for d in arange(RESOLUTION, dist + 0.5, RESOLUTION):
pose.translation.x = (self.next_pose.position.x -
x) * (d / dist) + x
pose.translation.y = (self.next_pose.position.y -
y) * (d / dist) + y
pose.translation.z = (self.next_pose.position.z -
z) * (d / dist) + z
self.current_pose.multi_dof_joint_state.transforms[
0] = pose # Insert the correct odometry value
validity_msg.robot_state = self.current_pose
# Call service to verify collision
collision_res = self.validity_srv.call(validity_msg)
# print("\nCollision response:")
# print(collision_res)
# Check if robot is in collision
if not collision_res.valid:
# print(validity_msg)
rospy.logerr('Collision in front [x:{} y:{} z:{}]'.format(
pose.translation.x, pose.translation.y,
pose.translation.z))
rospy.logerr('Current pose [x:{} y:{} z:{}]'.format(
x, y, z))
# print(collision_res)
self.move_client.cancel_goal()
self.collision = True
return
def collision_check(self, joint_angles):
gsvr = GetStateValidityRequest()
self.rs.joint_state.position = joint_angles
gsvr.robot_state = self.rs
gsvr.group_name = self.group_name
result = self.sv_srv.call(gsvr)
return result
def getStateValidity(self, robot_state, group_name='both_arms_torso', constraints=None):
"""Given a RobotState and a group name and an optional Constraints
return the validity of the State"""
gsvr = GetStateValidityRequest()
gsvr.robot_state = robot_state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = self.sv_srv.call(gsvr)
#GetStateValidityResponse()
#rospy.logwarn("sent: " + str(gsvr))
return result
def is_state_valid(self, q):
req = GetStateValidityRequest()
req.group_name = self.group_name
# current_joint_state = deepcopy(self.joint_state)
# current_joint_state.position = q
# current_joint_state.name = self.joint_names
# self.joint_state_from_q(current_joint_state, q)
req.robot_state = RobotState()
req.robot_state.joint_state.position = q
req.robot_state.joint_state.name = self.joint_names
res = self.state_valid_service(req)
return res.valid
def getStateValidity(self, robot_state, group_name='both_arms_torso', constraints=None):
"""Given a RobotState and a group name and an optional Constraints
return the validity of the State"""
gsvr = GetStateValidityRequest()
gsvr.robot_state = robot_state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = self.sv_srv.call(gsvr)
# if not result.valid:
# print("The contact information are: %s" % result.contacts)
# print("After checking for state validity. Result: %s" % result.valid)
return result
def getStateValidity(self,
robot_state,
group_name='both_arms_torso',
constraints=None):
"""Given a RobotState and a group name and an optional Constraints
return the validity of the State"""
gsvr = GetStateValidityRequest()
gsvr.robot_state = robot_state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = self.sv_srv.call(gsvr)
return result.valid
def getStateValidity(self, group_name='acrobat', constraints=None):
'''
Given a RobotState and a group name and an optional Constraints
return the validity of the State
'''
gsvr = GetStateValidityRequest()
gsvr.robot_state = self.rs
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = self.sv_srv.call(gsvr)
print(result)
return result
def getStateValidity(self, robot_state, group_name='both_arms_torso', constraints=None):
"""Given a RobotState and a group name and an optional Constraints
return the validity of the State"""
gsvr = GetStateValidityRequest()
gsvr.robot_state = robot_state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = self.sv_srv.call(gsvr)
# GetStateValidityResponse()
# rospy.logwarn("sent: " + str(gsvr))
# JAN BEHRENS ([email protected] - 2018-10-30): Return a bool instead of the full message
return result.valid
def get_state_validity(state, group_name="robot_arm"):
rospy.wait_for_service('/check_state_validity')
sv_srv = rospy.ServiceProxy('/check_state_validity', GetStateValidity)
gsvr = GetStateValidityRequest()
gsvr.robot_state = state
gsvr.group_name = group_name
result = sv_srv.call(gsvr)
# if not result.valid:
# print('Collision Checker failed.')
# print('contact: ')
# for i in range(len(result.contacts)):
# print('contact_body_1: %s, type: %d' % (result.contacts[i].contact_body_1, result.contacts[i].body_type_1))
# print('contact_body_2: %s, type: %d' % (result.contacts[i].contact_body_2, result.contacts[i].body_type_2))
return result
def _check_trajectory_validity(self,
robot_trajectory,
groups=['left_arm']):
for traj_point in robot_trajectory.joint_trajectory.points:
rs = RobotState()
rs.joint_state.name = robot_trajectory.joint_trajectory.joint_names
rs.joint_state.position = traj_point.positions
for group in groups:
gsvr = GetStateValidityRequest()
gsvr.robot_state = rs
gsvr.group_name = group
result = self.robot_controller.collision_proxy.call(gsvr)
for contact in result.contacts:
if 'table' in contact.contact_body_1 or \
'testObject2' in contact.contact_body_1 or \
'table' in contact.contact_body_2 or \
'testObject2' in contact.contact_body_2:
return False
return True
def getStateValidity(self, robot_state, group_name='both_arms_torso', constraints=None, print_depth=False):
"""Given a RobotState and a group name and an optional Constraints
return the validity of the State"""
gsvr = GetStateValidityRequest()
gsvr.robot_state = robot_state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = self.sv_srv.call(gsvr)
if (not result.valid):
contact_depths = []
for i in range(len(result.contacts)):
contact_depths.append(result.contacts[i].depth)
max_depth = max(contact_depths)
if max_depth < 0.0001:
return True
else:
return False
return result.valid
def call(self, robot_state, group_name="right_arm"):
"""
Given a robot state and a group name, calculate whether or not the state is valid.
Parameters
----------
robot_state : RobotState
RobotState for which to check validity.
group_name : string
Name of the group (ex: "right_arm") on which to check for validity.
Returns
-------
response : GetStateValidityResponse
The GetStateValidityResponse response from the /check_state_validity service.
"""
request = GetStateValidityRequest()
request.robot_state = robot_state
request.group_name = group_name
try:
response = self.service.call(request)
return response.valid
except ServiceException as e:
rospy.logwarn(e)
return None
def getStateValidity(self, group_name='ur5', constraints=None):
'''
Given a RobotState and a group name and an optional Constraints
return the validity of the State
'''
gsvr = GetStateValidityRequest()
gsvr.robot_state = self.rs.state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
print("====================================")
print(self.rs.state.joint_state.position)
result = self.sv_srv.call(gsvr)
print('result:')
print('valid = {}'.format(result.valid))
rc = result.contacts
for i in range(len(rc)):
print('contact {}: 1-{}, 2-{}'.format(i, rc[i].contact_body_1,
rc[i].contact_body_2))
return result
def _get_state_validity(self, joint):
print(joint)
robot_state = moveit_msgs.msg.RobotState()
robot_state.joint_state.name = JOINTS
robot_state.joint_state.position = joint
if self.aco is not None:
robot_state.attached_collision_objects = [self.aco]
gsvr = GetStateValidityRequest()
gsvr.robot_state = robot_state
result = self.sv_srv.call(gsvr)
rv = result.valid
rc = result.contacts
print("+"*70)
print('state validity result')
print('valid = {}'.format(rv))
for i in range(len(rc)):
print('contact {}: 1-{}, 2-{}'.format(i, rc[i].contact_body_1, rc[i].contact_body_2))
return rv
torch.random.manual_seed(1917)
num_init_points = 8000
cfgs = torch.rand((num_init_points, DOF), dtype=torch.float32)
cfgs = cfgs * (robot.limits[:, 1] - robot.limits[:, 0]) + robot.limits[:,
0]
labels = torch.zeros(num_init_points, dtype=torch.float)
# dists = torch.zeros(num_init_points, dtype=torch.float)
# req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
rs = RobotState()
rs.joint_state.name = [
'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1',
'left_w2'
]
gsvr = GetStateValidityRequest()
gsvr.robot_state = rs
gsvr.group_name = group_name
for i, cfg in tqdm(enumerate(cfgs)):
rs.joint_state.position = cfg
result = sv_srv.call(gsvr)
in_collision = not result.valid
labels[i] = 1 if in_collision else -1
# if in_collision:
# depths = torch.tensor([c.penetration_depth for c in rdata.result.contacts])
# dists[i] = depths.abs().max()
# else:
# ddata = fcl.DistanceData()
# robot_manager.distance(obs_manager, ddata, fcl.defaultDistanceCallback)
# dists[i] = -ddata.result.min_distance
print('{} collisons, {} free'.format(torch.sum(labels == 1),