def publish_robot_state(robot_state_publisher,
state,
state_validity_checker=None,
duration=1.0,
display_status='all'):
robot_state = RobotState()
robot_state.joint_state = convertStateToJointState(
state) # convert this to a way-point first
# robot_state.is_diff = False
robot_state_msg = DisplayRobotState()
robot_state_msg.state = robot_state
if display_status != 'all':
result = state_validity_checker.getStateValidity(
robot_state, group_name='both_arms_torso', constraints=None)
# print("The result for validity check is: %s" % result)
if display_status == 'valid' and result.valid or (
display_status == 'inValid' and not result.valid):
robot_state_publisher.publish(robot_state_msg)
time.sleep(duration)
if not result.valid:
print("The result for validity check is: %s" % result)
return True
else:
robot_state_publisher.publish(robot_state_msg)
time.sleep(duration)
return True
return False
def checkTrajectoryValidity(self, robot_trajectory, groups=[]):
"""Given a robot trajectory, deduce it's groups and check it's validity on each point of the traj
returns True if valid, False otherwise
It's considered not valid if any point is not valid"""
#r = RobotTrajectory()
init_time = time.time()
if len(groups) > 0:
groups_to_check = groups
else:
groups_to_check = [
'both_arms_torso'
] # Automagic group deduction... giving a group that includes everything
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_to_check:
result = self.sv.getStateValidity(rs, group) #, constraints)
if not result.valid: # if one point is not valid, the trajectory is not valid
rospy.logerr(
"Trajectory is not valid at point (RobotState):" +
str(rs) + "with result of StateValidity: " +
str(result))
rospy.logerr(
"published in /robot_collision_state the conflicting state"
)
drs = DisplayRobotState()
drs.state = rs
self.robot_state_collision_pub.publish(drs)
return False
fin_time = time.time()
rospy.logwarn("Trajectory validity of " +
str(len(robot_trajectory.joint_trajectory.points)) +
" points took " + str(fin_time - init_time))
return True
def checkTrajectoryValidity(self, robot_trajectory, groups=[]):
"""Given a robot trajectory, deduce it's groups and check it's validity on each point of the traj
returns True if valid, False otherwise
It's considered not valid if any point is not valid"""
#r = RobotTrajectory()
init_time = time.time()
if len(groups) > 0:
groups_to_check = groups
else:
groups_to_check = ['both_arms_torso'] # Automagic group deduction... giving a group that includes everything
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_to_check:
result = self.sv.getStateValidity(rs, group)#, constraints)
if not result.valid: # if one point is not valid, the trajectory is not valid
rospy.logerr("Trajectory is not valid at point (RobotState):" + str(rs) + "with result of StateValidity: " + str(result))
rospy.logerr("published in /robot_collision_state the conflicting state")
drs = DisplayRobotState()
drs.state = rs
self.robot_state_collision_pub.publish(drs)
return False
fin_time = time.time()
rospy.logwarn("Trajectory validity of " + str(len(robot_trajectory.joint_trajectory.points)) + " points took " + str(fin_time - init_time))
return True
def visualize_waypoint(limb,
waypoint,
robot_state_collision_pub,
publish_collision=False):
limb.move_to_joint_positions(
waypoint) # moves to waypointO for visual confirmation
if not publish_collision and limb is not None:
# Publish collision information
drs = DisplayRobotState()
drs.state = rs
robot_state_collision_pub.publish(drs)
def display_robot_state(robot_state, block=True):
display_msg = DisplayRobotState()
display_msg.state = robot_state
robot_state_pub = rospy.Publisher(
rospy.resolve_name('/display_robot_state'),
DisplayRobotState,
queue_size=10)
rospy.sleep(1.0) # allow publisher to initialize
robot_state_pub.publish(display_msg)
print("Publishing display message...")
if block:
raw_input("display input...")
def publish_joint_trajectories(self, *joint_trajectories):
display_trajectory = self.get_display_trajectory(*joint_trajectories)
display_state = DisplayRobotState()
display_state.state = display_trajectory.trajectory_start
# self.robot_state_pub.publish(display_state)
self.display_trajectory_pub.publish(display_trajectory)
# raw_input('Continue?')
last_trajectory = joint_trajectories[-1]
last_conf = last_trajectory.points[-1].positions
joint_state = display_state.state.joint_state
joint_state.position = list(joint_state.position)
for joint_name, position in zip(last_trajectory.joint_names,
last_conf):
joint_index = joint_state.name.index(joint_name)
joint_state.position[joint_index] = position
self.robot_state_pub.publish(display_state)
# TODO: record executed trajectory and overlay them
return display_trajectory
def loop(self):
hz = get_param("rate", 10) # 10hz
r = rospy.Rate(hz)
delta = get_param("delta", 0.0)
msg = JointState()
msgDisplayRobotState = DisplayRobotState()
msgRobotState = RobotState()
# Publish Joint States
while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
if delta > 0:
self.update(delta)
# Initialize msg.position, msg.velocity, and msg.effort.
has_position = len(self.dependent_joints.items()) > 0
has_velocity = False
has_effort = False
for (name,joint) in self.free_joints.items():
if not has_position and 'position' in joint:
has_position = True
if not has_velocity and 'velocity' in joint:
has_velocity = True
if not has_effort and 'effort' in joint:
has_effort = True
num_joints = (len(self.free_joints.items()) +
len(self.dependent_joints.items()))
if has_position:
msg.position = num_joints * [0.0]
if has_velocity:
msg.velocity = num_joints * [0.0]
if has_effort:
msg.effort = num_joints * [0.0]
for i, name in enumerate(self.joint_list):
msg.name.append(str(name))
joint = None
# Add Free Joint
if name in self.free_joints:
joint = self.free_joints[name]
factor = 1
offset = 0
# Add Dependent Joint
elif name in self.dependent_joints:
param = self.dependent_joints[name]
parent = param['parent']
joint = self.free_joints[parent]
factor = param.get('factor', 1)
offset = param.get('offset', 0)
if has_position and 'position' in joint:
pos = self.joint_positions[ name ]
msg.position[i] = pos
if has_velocity and 'velocity' in joint:
msg.velocity[i] = joint['velocity'] * factor
if has_effort and 'effort' in joint:
msg.effort[i] = joint['effort']
msgRobotState.joint_state = msg
msgDisplayRobotState.state = msgRobotState
self.pub.publish( msgDisplayRobotState )
r.sleep()
random_js = copy.deepcopy(current_joint_states)
positions_list = []
for id, item in enumerate(current_joint_states_modif.position):
positions_list.append(random.random() * 3.14)
random_js.position = positions_list
# c = Constraints()
# jc = JointConstraint()
# jc.joint_name = 'arm_right_1_link'
# c.joint_constraints.append()
result = fk.getFK('arm_right_7_link', current_joint_states.name, random_js.position, 'base_link')
rospy.loginfo("Result of a disastrous robot pose is: " + str(result))
rs = RobotState()
rs.joint_state = random_js
drs = DisplayRobotState()
drs.state = rs
rs_pub.publish(drs)
rospy.loginfo("Published robot state")
rospy.loginfo("Asking for IK with collision avoidance for the pose given")
print "Result is:"
print result
print "And result.pose_stamped is:"
print result.pose_stamped
print "with result.pose_stamped[0]"
print result.pose_stamped[0]
resultik = ik.getIK("right_arm", "right_arm_7_link", result.pose_stamped[0], True, 0, rs)
rospy.loginfo(str(resultik))
rospy.sleep(1.0)
# rospy.loginfo("isJointConfigInCollision with all left arm at 0.0 (should be False):")