def setUp(self):
self.env, self.robot = herbpy.initialize(sim=True)
self.fuze = prpy.rave.add_object(self.env, 'fuze_bottle', 'objects/fuze_bottle.kinbody.xml')
self.robot.right_arm.SetActive()
self.fuze_pose = numpy.eye(4)
self.fuze_pose[0:3, 3] = [0.8, -0.3, 0.4]
self.fuze.SetTransform(self.fuze_pose)
self.fuze.Enable(True)
def setUp(self):
self.env, self.robot = herbpy.initialize(sim=True)
self.fuze = prpy.rave.add_object(self.env, 'fuze_bottle',
'objects/fuze_bottle.kinbody.xml')
self.robot.right_arm.SetActive()
self.fuze_pose = numpy.eye(4)
self.fuze_pose[0:3, 3] = [0.8, -0.3, 0.4]
self.fuze.SetTransform(self.fuze_pose)
self.fuze.Enable(True)
def setup_env(isReal, attach_viewer=True):
env, robot = herbpy.initialize(sim=not isReal, attach_viewer=attach_viewer)
if isReal:
robot.left_arm.SetVelocityLimits(np.ones(7)*1.2, 1.2)
table = add_table(env, robot)
bin = add_bin(env)
return env, robot, table, bin
def main():
#import IPython
#IPython.embed()
env, robot = herbpy.initialize(sim=True, attach_viewer='interactivemarker')
planning_env = HerbEnv(env, robot)
obj_list = planning_env.obj_list
import IPython
IPython.embed()
plan = Planner(env, robot, planning_env)
plan.Plan(obj_list)
time.sleep(10000)
def main():
#import IPython
#IPython.embed()
env, robot = herbpy.initialize(sim=True, attach_viewer='interactivemarker')
planning_env = HerbEnv(env, robot)
obj_list = planning_env.obj_list
import IPython
IPython.embed()
plan = Planner(env, robot, planning_env)
plan.Plan(obj_list)
time.sleep(10000)
def main():
sim = True
env, robot = herbpy.initialize(sim=sim, segway_sim=True)
#robot.right_arm.SetActive()
robot.head.SetStiffness(1)
robot.head.MoveTo([0, -0.4])
robot.SetActiveManipulator(robot.right_arm)
env.SetViewer('qtcoin')
env.GetViewer().SetName('HPN Viewer')
env.Add(robot)
kinbody_env = HerbEnv(env, robot, sim)
import IPython
IPython.embed()
plan = Planner(env, robot, kinbody_env)
plan.Plan(kinbody_env.obj_list)
def main():
sim = True
env, robot = herbpy.initialize(sim=sim,segway_sim=True)
#robot.right_arm.SetActive()
robot.head.SetStiffness(1)
robot.head.MoveTo([0,-0.4])
robot.SetActiveManipulator(robot.right_arm)
env.SetViewer('qtcoin')
env.GetViewer().SetName('HPN Viewer')
env.Add(robot)
kinbody_env = HerbEnv(env,robot,sim)
import IPython
IPython.embed()
plan = Planner(env,robot,kinbody_env)
plan.Plan(kinbody_env.obj_list)
def run_benchmark(body, engine, testfile=None, self_collision=False, envfile=None,
random=50000, extent=2.0, viewer=None, outfile=None ):
# Load the environment
if body == 'herb':
env, robot = herbpy.initialize(sim=True, attach_viewer=viewer)
else:
env = openravepy.Environment()
if envfile:
env.Load(envfile)
# Set the collision checker
cc = openravepy.RaveCreateCollisionChecker(env, engine)
cc.SetCollisionOptions(0)
if cc is None:
raise Exception('Invalid collision engine. Failing.')
env.SetCollisionChecker(cc)
# Verify the kinbody is in the environment
body = env.GetKinBody(body)
if body is None:
raise Exception('No body with name %s in environment. Failing.'
% body)
# Load the openrave module
try:
module = openravepy.RaveCreateModule(env, 'collisioncheckingbenchmark')
except openravepy.openrave_exception:
raise Exception('Unable to load CollisionCheckingBenchmark module.'
' Check your OPENRAVE_PLUGINS environment variable.')
# Generate the parameters
params = {}
params['body'] = str(body.GetName())
params['random'] = random
if outfile is not None:
params['outfile'] = outfile
params['extent'] = extent
if self_collision:
params['self'] = True
if testfile:
params['datafile'] = testfile
with env:
result = module.SendCommand("Run " + str(params))
num_non_adjacent_links = len(non_adjacent_links)
actual_num_link_pairs = num_adjacent_links + num_non_adjacent_links
print "robot.GetAdjacentLinks() = ", robot.GetAdjacentLinks()
print "robot.GetNonAdjacentLinks() = ", robot.GetNonAdjacentLinks()
if expected_num_link_pairs == actual_num_link_pairs:
print ("\nLink pairs are correct\n")
else:
print ("\nERROR: some link pairs are missing\n")
# Initialize OpenRAVE robot of type herbpy.herbrobot.HERBRobot:
# env, robot = herbpy.initialize(sim=True, attach_viewer=True)
# env, robot = herbpy.initialize(sim=True, attach_viewer='qtcoin')
env, robot = herbpy.initialize(sim=True, attach_viewer="rviz")
# env, robot = herbpy.initialize(sim=True, attach_viewer='interactivemarker')
# Load the fridge (trimesh only)
current_dir = os.path.dirname(os.path.abspath(__file__))
# fridge_file = os.path.join(current_dir, 'prkitchen_refrigerator.robot.xml')
fridge_file = os.path.join(current_dir, "fridge_trimesh.robot.xml")
env.Load(fridge_file)
fridge = env.GetKinBody("refrigerator")
CheckAdjacentLinks(fridge)
# Delete the fridge
env.Remove(env.GetKinBody("refrigerator"))
else:
angle_measurement = abs(max_measurement[iaxis] - min_measurement[iaxis])
if angle_measurement - angle_actual > math.pi:
angle_measurement = angle_measurement - 2 * math.pi
elif angle_measurement - angle_actual < -math.pi:
angle_measurement = angle_measurement + 2 * math.pi
return angle_actual, angle_measurement
if __name__ == '__main__':
sim = False
namespace = '/left/owd/'
output_path = 'transmission_ratios.yaml'
env, robot = herbpy.initialize(sim=sim, segway_sim=True, head_sim=True, right_arm_sim=True, right_hand_sim=True, attach_viewer='interactivemarker')
robot.planner = prpy.planning.SnapPlanner()
# TODO: Hack to work around a race condition in or_interactivemarker.
import time
time.sleep(0.1)
manipulator = robot.left_arm
robot.SetActiveDOFs(manipulator.GetArmIndices())
# Pad the joint limits to avoid hitting the floor and head.
min_limit, max_limit = robot.GetDOFLimits()
arm_indices = manipulator.GetArmIndices()
min_limit[arm_indices[1]] = -0.5
max_limit[arm_indices[1]] = 0.5
max_limit[arm_indices[3]] = 1.5
import herbpy
import numpy
import unittest
env, robot = herbpy.initialize(sim=True)
class BarrettHandTest(unittest.TestCase):
def setUp(self):
self._env, self._robot = env, robot
self._wam = robot.right_arm
self._hand = self._wam.hand
self._indices = self._wam.GetChildDOFIndices()
self._num_dofs = len(self._indices)
def test_GetIndices_ReturnsIndices(self):
indices = self._wam.GetChildDOFIndices()
numpy.testing.assert_array_equal(
sorted(self._hand.GetIndices()), sorted(indices))
def test_GetDOFValues_SetsValues(self):
expected_values = numpy.array([0.1, 0.2, 0.3, 0.4])
self._robot.SetDOFValues(expected_values, self._hand.GetIndices())
numpy.testing.assert_array_almost_equal(
self._hand.GetDOFValues(), expected_values)
def test_SetDOFValues_SetsValues(self):
expected_values = numpy.array([ 0.1, 0.2, 0.3, 0.4 ])
self._hand.SetDOFValues(expected_values)
numpy.testing.assert_array_almost_equal(
self._robot.GetDOFValues(self._hand.GetIndices()), expected_values)
#!/usr/bin/env python
import herbpy
import numpy
if __name__ == '__main__':
env, robot = herbpy.initialize(sim=False)
env.SetViewer('interactivemarker')
keep_going = True
while keep_going:
print "\n\nWelcome to the pose control:\n \
Press 0 to reset to relaxed home.\n \
Press 1 to wave.\n \
Press 2 to say something.\n \
Press 3 for the Rock On pose.\n \
Press 4 for the Right Arm Hug pose.\n \
Press 5 for the Huzzah pose.\n \
Press 6 for the Samba pose.\n \
Press 9 to quit.\n"
user_input = int(raw_input("Gesture? "))
if user_input == 9:
print 'Goodbye!\n'
keep_going = False
elif user_input == 1:
print 'Waving!\n'
robot.right_arm.SetActive()
robot.Wave()
elif user_input == 0:
#!/usr/bin/env python
import numpy
import logging
import herbpy
import humanpy
if __name__ == "__main__":
logger = logging.getLogger("test_humherb")
logger.setLevel(logging.INFO)
sim = True
env, herb = herbpy.initialize(attach_viewer=True, sim=sim)
with env:
_, human = humanpy.initialize(sim=sim, env=env)
humanLocation = numpy.array(
[[0.0, 0.0, -1.0, 1.9], [-1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.85], [0.0, 0.0, 0.0, 1.0]] # -1.2 # -0.5
)
human.SetTransform(humanLocation)
human.right_arm.SetActive()
table = env.ReadKinBodyXMLFile("objects/table.kinbody.xml")
table_pose = numpy.array(
[[0.0, 0.0, 1.0, 1.1], [1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]]
)
table.SetTransform(table_pose)
table.SetName("table")
env.AddKinBody(table)
table_aabb = table.ComputeAABB()
[ 3.1670137672513565, -1.5896855774682224, -0.030490730254705722, 1.5286675450710463, -0.032386115821109854, -0.03605547869739324, -0.026511110707008654 ],
[ 5.147064052327087, -1.3622957134787639, -0.46951207447023835, 1.4621965585328196, -0.6525084419130015, 0.9042586427293137, -1.00526432576227 ],
[ 4.121337486391367, 0.5703433599319374, -2.2175569328355724, 2.049783564999927, -0.2955432490571725, -0.646366247533136, -1.4040613050409545 ],
[ 3.102831025390782, -0.04296665515610201, 0.1361467570187895, 0.045580105054784076, -3.0213214848284657, 0.0714728073293458, -0.24661498332101073 ]
]
for i,jpos in enumerate(jpos_list):
robot.SetActiveDOFValues(jpos, openravepy.KinBody.CheckLimitsAction.Nothing)
robot.SetActiveDOFVelocities(numpy.zeros(7))
jtor = robot.ComputeInverseDynamics([])
print('jpos[{}] gravtorques: {}'.format(i, ' '.join(['{:7.3f}'.format(x) for x in jtor[robot.GetActiveDOFIndices()]])))
if __name__ == "__main__":
openravepy.RaveInitialize(True)
openravepy.misc.InitOpenRAVELogging()
# herbpy_args = {'sim':args.sim,
# 'attach_viewer':args.viewer,
# 'robot_xml':args.robot_xml,
# 'env_path':args.env_xml,
# 'segway_sim':args.segway_sim}
# if args.sim and not args.segway_sim:
# herbpy_args['segway_sim'] = args.sim
env, robot = herbpy.initialize(sim=True, segway_sim=True)
iD()
#!/usr/bin/env python
PKG = 'herbpy'
import roslib; roslib.load_manifest(PKG)
import numpy, unittest
import herbpy
env, robot = herbpy.initialize(sim=True)
class WamTest(unittest.TestCase):
def setUp(self):
self._env, self._robot = env, robot
self._wam = robot.right_arm
self._indices = self._wam.GetArmIndices()
self._num_dofs = len(self._indices)
def test_SetStiffness_DoesNotThrow(self):
self._wam.SetStiffness(0.0)
self._wam.SetStiffness(0.5)
self._wam.SetStiffness(1.0)
def test_SetStiffness_InvalidStiffnessThrows(self):
self.assertRaises(Exception, self._wam.SetStiffness, (-0.2,))
self.assertRaises(Exception, self._wam.SetStiffness, ( 1.2,))
def test_Servo_DoesNotThrow(self):
self._wam.Servo(0.1 * numpy.ones(self._num_dofs))
def test_Servo_IncorrectSizeThrows(self):
velocity_small = 0.1 * numpy.ones(self._num_dofs - 1)
velocity_large = 0.1 * numpy.ones(self._num_dofs + 1)
self.assertRaises(Exception, self._wam.Servo, (velocity_small,))
if angle_measurement - angle_actual > math.pi:
angle_measurement = angle_measurement - 2 * math.pi
elif angle_measurement - angle_actual < -math.pi:
angle_measurement = angle_measurement + 2 * math.pi
return angle_actual, angle_measurement
if __name__ == '__main__':
sim = False
namespace = '/left/owd/'
output_path = 'transmission_ratios.yaml'
env, robot = herbpy.initialize(sim=sim,
segway_sim=True,
head_sim=True,
right_arm_sim=True,
right_hand_sim=True,
attach_viewer='interactivemarker')
robot.planner = prpy.planning.SnapPlanner()
# TODO: Hack to work around a race condition in or_interactivemarker.
import time
time.sleep(0.1)
manipulator = robot.left_arm
robot.SetActiveDOFs(manipulator.GetArmIndices())
# Pad the joint limits to avoid hitting the floor and head.
min_limit, max_limit = robot.GetDOFLimits()
arm_indices = manipulator.GetArmIndices()
min_limit[arm_indices[1]] = -0.5
#!/usr/bin/env python
import herbpy
import numpy
if __name__ == '__main__':
env, robot = herbpy.initialize()
keep_going = True
while keep_going:
print "\n\nWelcome to the gesture control:\n \
Press W to wave.\n \
Press S to say something.\n \
Press Y to nod 'yes'.\n \
Press N to nod 'no'.\n \
Press H to high five.\n \
Press P to point at an object.\n \
Press S to present an object.\n \
Press R to reset the robot.\n \
Press Q to quit.\n"
user_input = raw_input("Gesture? ")
if user_input == 'Q':
print 'Goodbye!\n'
keep_going = False
elif user_input == 'W':
print 'Waving!\n'
robot.Wave()
elif user_input == 'Y':
print 'Nodding yes!\n'
robot.Nod(word='yes')
path=directory,
first_match_only=True)
if len(objects_path) == 0:
print 'Can\'t find directory {}/{}'.format(package_name, directory)
sys.exit()
else:
print objects_path
# for me this is:
#'/home/USERNAME/catkin_workspaces/herb_ws/src/pr-ordata/data/objects'
objects_path = objects_path[0]
# ===========================
# ENVIRONMENT SETUP
# ===========================
env, robot = herbpy.initialize(sim=True, attach_viewer='rviz')
# add a table to the environment
table_file = os.path.join(objects_path, 'table.kinbody.xml')
table = env.ReadKinBodyXMLFile(table_file)
if table == None:
print 'Failed to load table kinbody'
sys.exit()
env.AddKinBody(table)
table_pose = numpy.array([[1., 0., 0., 2], [0., 0., -1., 2], [0., 1., 0., 0.0],
[0., 0., 0., 1.]])
table.SetTransform(table_pose)
# add a fuze bottle on top of the table
fuze_path = os.path.join(objects_path, 'fuze_bottle.kinbody.xml')
fuze = env.ReadKinBodyXMLFile(fuze_path)
from catkin.find_in_workspaces import find_in_workspaces
package_name = 'pr_ordata'
directory = 'data/objects'
objects_path = find_in_workspaces(
search_dirs=['share'],
project=package_name,
path=directory,
first_match_only=True)
if len(objects_path) == 0:
print('Can\'t find directory %s %s' % (package_name, directory))
sys.exit()
else:
print objects_path
objects_path = objects_path[0]
env, robot = herbpy.initialize(sim=True, attach_viewer='rviz',segway_sim=True)
#Add the table
table_file = os.path.join(objects_path, 'table.kinbody.xml')
table = env.ReadKinBodyXMLFile(table_file)
#env.AddKinBody(table)
if table == None:
print('Failed to load table kinbody')
sys.exit()
env.AddKinBody(table)
table_pose = np.array([[1., 0., 0., 2],
[0., 0., -1., 2],
[0., 1., 0., 0.],
[0., 0., 0., 1.]])
table.SetTransform(table_pose)
logger = logging.getLogger(__name__)
if __name__ == '__main__':
import argparse
parser = argparse.ArgumentParser(description="Demonstrate using push planner to make object graspable")
parser.add_argument("-v", "--viewer", dest="viewer", default=None,
help="The viewer to attach")
parser.add_argument("-d", "--debug", action="store_true",
help="Run in debug mode")
parser.add_argument("-t", "--timeout", default=120.0, type=float,
help="The max time to run the planner")
args = parser.parse_args()
import herbpy
env, robot = herbpy.initialize(sim=True, attach_viewer=args.viewer)
push_arm = robot.right_arm
grasp_arm = robot.left_arm
# Put the hand in a preshape for pushing
push_arm.hand.MoveHand(spread=0, f1=0.75, f2=0.75, f3=0.75)
# Table
import os
table_path = os.path.join('objects', 'table.kinbody.xml')
table = env.ReadKinBodyXMLFile(table_path)
import numpy, openravepy
table_transform = numpy.eye(4)
table_transform[:3, :3] = openravepy.rotationMatrixFromAxisAngle([1.20919958, 1.20919958, 1.20919958])
check = classify_ee_pos(eepos, table_pose)
# 1 -> valid start_pos, 2 -> valid goal_pos, 0 -> invalid_pos
if (check == 1 and len(start) < 50):
start.append(random_node)
elif (check == 2 and len(goal) < 50):
goal.append(random_node)
if (len(start) == 50 and len(goal) == 50):
write_one_dir(task_id, env_no, start, goal, cond, binary_vec)
else:
print("no of start or goal posiitons is under limit")
def generate_data(task_id, robot, env, G):
if (task_id == "T1"):
object_around_table(task_id, robot, env, G)
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Generate environments')
parser.add_argument('--graphfile', type=str, required=True)
args = parser.parse_args()
env, robot = herbpy.initialize(sim=True, attach_viewer='interactivemarker')
robot.right_arm.SetActive()
G = nx.read_graphml(args.graphfile)
generate_data("T1", robot, env, G)
nx.write_graphml(G, "graphs/weighted_graph.graphml")
]]
for i, jpos in enumerate(jpos_list):
robot.SetActiveDOFValues(jpos,
openravepy.KinBody.CheckLimitsAction.Nothing)
robot.SetActiveDOFVelocities(numpy.zeros(7))
jtor = robot.ComputeInverseDynamics([])
print 'jpos[{}] gravtorques: {}'.format(
i, ' '.join([
'{:7.3f}'.format(x) for x in jtor[robot.GetActiveDOFIndices()]
]))
if __name__ == "__main__":
openravepy.RaveInitialize(True)
openravepy.misc.InitOpenRAVELogging()
# herbpy_args = {'sim':args.sim,
# 'attach_viewer':args.viewer,
# 'robot_xml':args.robot_xml,
# 'env_path':args.env_xml,
# 'segway_sim':args.segway_sim}
# if args.sim and not args.segway_sim:
# herbpy_args['segway_sim'] = args.sim
env, robot = herbpy.initialize(sim=True, segway_sim=True)
iD()
help='robot XML file; defaults to herb_description')
parser.add_argument('--env-xml', type=str,
help='environment XML file; defaults to an empty environment')
parser.add_argument('-b', '--segway-sim', action='store_true',
help='simulate base')
parser.add_argument('-p', '--perception-sim', action='store_true',
help='simulate perception')
parser.add_argument('--debug', action='store_true',
help='enable debug logging')
args = parser.parse_args()
openravepy.RaveInitialize(True)
openravepy.misc.InitOpenRAVELogging()
if args.debug:
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Debug)
herbpy_args = {'sim':args.sim,
'attach_viewer':args.viewer,
'robot_xml':args.robot_xml,
'env_path':args.env_xml,
'segway_sim':args.segway_sim,
'perception_sim': args.perception_sim}
if args.sim and not args.segway_sim:
herbpy_args['segway_sim'] = args.sim
env, robot = herbpy.initialize(**herbpy_args)
import IPython
IPython.embed()
service = rospy.ServiceProxy(service_name, DetectPlane);
response = service(cloud_topic);
return response
except rospy.ServiceException, e:
print "Service call failed: %s"%e
rospy.init_node('table_detector',anonymous=True)
from catkin.find_in_workspaces import find_in_workspaces
objects_path = find_in_workspaces(
search_dirs=['share'],
project='pr_ordata',
path='data/test',
first_match_only=True)[0]
env, robot = herbpy.initialize(sim=True,attach_viewer='interactivemarker')
plane_file = os.path.join(objects_path, 'plane.xml')
plane = env.ReadKinBodyXMLFile(plane_file)
plane_resp = find_table_plane()
plane_pose = numpy.array(quaternion_matrix([
plane_resp.pose.orientation.x,
plane_resp.pose.orientation.y,
plane_resp.pose.orientation.z,
plane_resp.pose.orientation.w]))
#plane_pose = numpy.eye(4)
plane_pose[0,3] = plane_resp.pose.position.x
plane_pose[1,3] = plane_resp.pose.position.y
def main():
"""Execute demo for grasping glass."""
parser = argparse.ArgumentParser()
parser.add_argument('--viewer',
'-v',
type=str,
default='interactivemarker',
help='The viewer to attach (none for no viewer)')
parser.add_argument('--monitor',
action='store_true',
help='Display a UI to monitor progress of the planner')
parser.add_argument('--planner',
type=str,
choices=['dfs', 'restart'],
default='restart',
help='The planner to use')
parser.add_argument('--robot',
type=str,
default='herb',
help='Robot to run the task on')
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
openravepy.misc.InitOpenRAVELogging()
args = parser.parse_args()
env, robot = herbpy.initialize()
# Get the desired manipulator
manipulator = robot.GetManipulator('right')
if args.viewer != 'none':
env.SetViewer(args.viewer)
monitor = None
# Create a monitor
if args.monitor:
monitor = magi.monitor.ActionMonitor()
def signal_handler(signum, frame):
"""Signal handler to gracefully kill the monitor."""
monitor.stop()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
signal.signal(signal.SIGTERM, signal_handler)
# Create a planner
if args.planner == 'restart':
planner = RestartPlanner(monitor=monitor)
elif args.planner == 'dfs':
planner = DepthFirstPlanner(monitor=monitor, use_frustration=True)
if monitor is not None:
monitor.reset()
# Detect objects
table, glass = detect_objects(robot)
try:
# Create the task.
action = grasp_glass_action_graph(manipulator, glass, table)
# Plan the task
with env:
solution = planner.plan_action(env, action)
# Execute the task
execute_pipeline(env, solution, simulate=True, monitor=monitor)
except ActionError as err:
LOGGER.info('Failed to complete planning for task: %s', str(err))
raise
except ExecutionError as err:
LOGGER.info('Failed to execute task: %s', str(err))
raise
IPython.embed()
if monitor:
monitor.stop()
def main():
parser = argparse.ArgumentParser(description='Generate environments')
parser.add_argument('--condnsfile', type=str, required=True)
parser.add_argument('--graphfile', type=str, required=True)
args = parser.parse_args()
G = nx.read_graphml(args.graphfile)
env, robot = herbpy.initialize(sim=True, attach_viewer='interactivemarker')
robot.right_arm.SetActive()
# Load table from pr_ordata
table_file = os.path.join(objects_path, 'objects/table.kinbody.xml')
table = env.ReadKinBodyXMLFile(table_file)
env.AddKinBody(table)
xpos, ypos = get_table_pose(args.condnsfile)
pp_no = 5
table_pose[0, 3] = xpos
table_pose[1, 3] = ypos
table.SetTransform(table_pose)
count = 0
topic = 'output_node_pos'
publisher = rospy.Publisher(topic, MarkerArray)
topic = 'expected_node_pos'
publisher1 = rospy.Publisher(topic, MarkerArray)
rospy.init_node('output_node_pos')
markerArray = MarkerArray()
p_file_addr = "output_data/output_samples.txt"
eepositions = get_eepositions_from_samples(env, robot, p_file_addr)
path_node_file_addr = "output_data/expected_path_nodes.txt"
markerArray1 = get_markers_expected_samples(G, env, robot,
path_node_file_addr)
while not rospy.is_shutdown():
markerArray = MarkerArray()
eepositions, markerArray = load_marker(eepositions, markerArray)
print("len(eepositions) = ", len(eepositions))
id = 0
for m in markerArray.markers:
m.id = id
id += 1
# Publish the MarkerArray
publisher.publish(markerArray)
for m in markerArray1.markers:
m.id = id
id += 1
publisher1.publish(markerArray1)
# print("len(markerArray1) = ", len(markerArray1))
count += 1
rospy.sleep(0.1)
if args.debug:
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Debug)
herbpy_args = {
'sim': args.sim,
'attach_viewer': args.viewer,
'robot_xml': args.robot_xml,
'env_path': args.env_xml,
'segway_sim': args.segway_sim,
'perception_sim': args.perception_sim
}
if args.sim and not args.segway_sim:
herbpy_args['segway_sim'] = args.sim
env, robot = herbpy.initialize(**herbpy_args)
#Add in canvas cube
from openravepy import *
body = RaveCreateKinBody(env, '')
body.SetName('testbody')
body.InitFromBoxes(numpy.array(
[[0.6, 0, 1, 0.01, 0.2,
0.3]]), True) # set geometry as one box of extents 0.1, 0.2, 0.3
env.AddKinBody(body)
#Todo make center expressed in meters
dm = DrawingManager(robot, robot.right_arm, numpy.array([1, 0, 0]),
numpy.array([0.6, 0, 1]), numpy.array([11.5, 9]), body)
import IPython
IPython.embed()
#!/usr/bin/env python
import herbpy
import numpy
if __name__ == '__main__':
env, robot = herbpy.initialize()
keep_going = True
while keep_going:
print "\n\nWelcome to the gesture control:\n \
Press W to wave.\n \
Press S to say something.\n \
Press Y to nod 'yes'.\n \
Press N to nod 'no'.\n \
Press H to high five.\n \
Press P to point at an object.\n \
Press S to present an object.\n \
Press R to reset the robot.\n \
Press Q to quit.\n"
user_input = raw_input("Gesture? ")
if user_input == 'Q':
print 'Goodbye!\n'
keep_going = False
elif user_input == 'W':
print 'Waving!\n'
robot.Wave()
elif user_input == 'Y':
print 'Nodding yes!\n'
help="The environment to load")
parser.add_argument("--robot", type=str, default="herb",
help="The robot to use for the FK test")
parser.add_argument("--manip", type=str, default="right",
help="The manipulator to use for the FK test")
parser.add_argument("--random", type=int, default=50000,
help="The number of random poses to check. This will be ignored if the test parameters is set.")
parser.add_argument("--viewer", type=str, default=None,
help="The viewer to attach to the environment")
args = parser.parse_args()
# Load the environment
if args.robot == 'herb':
env, robot = herbpy.initialize(sim=True, attach_viewer=args.viewer)
else:
env = openravepy.Environment()
if args.env:
env.Load(args.env)
# Verify the kinbody is in the environment
body = env.GetKinBody(args.robot)
if body is None:
raise Exception('No robot with name %s in environment. Failing.' % args.robot)
# Load the openrave module
try:
module = openravepy.RaveCreateModule(env, 'kinematicbenchmarks')
except openravepy.openrave_exception:
#!/usr/bin/env python
import logging
import rospy
import herbpy
import humanpy.humandetection as humdet
if __name__ == "__main__":
logger = logging.getLogger('test_skel_herb')
logger.setLevel(logging.INFO)
rospy.init_node('humkin2')
segway_sim = rospy.get_param("~seg_sim");
env, herb = herbpy.initialize(attach_viewer='InteractiveMarker', segway_sim=segway_sim)
if segway_sim==True:
refsys = '/head/skel_depth_frame2'
else:
refsys = '/head/skel_depth_frame'
humdet.DetectHuman(env, orhuman='kin2_or', segway_sim=segway_sim, kin_frame=refsys)