def __init__(self, id, env,
base_frame='/map',
kin_frame='/head/skel_depth_frame',
**kw_args):
assert id != ''
self.id = id
self.enabled = True
self.env = env
self.base_frame = base_frame
self.kin_frame = kin_frame
_,self.body = humanpy.initialize(sim=False, user_id=id, env=env)
with env:
self.initialDOFvalues = self.body.GetDOFValues()
self.currentDOFvalues = self.body.GetDOFValues()
self.starting_angle = dict()
self.last_updated = rospy.get_rostime().secs
self.collshl = collections.deque(maxlen=MAXLEN)
self.collsh2l = collections.deque(maxlen=MAXLEN)
self.collsh3l = collections.deque(maxlen=MAXLEN)
self.collell = collections.deque(maxlen=MAXLEN)
self.collshr = collections.deque(maxlen=MAXLEN)
self.collsh2r = collections.deque(maxlen=MAXLEN)
self.collsh3r = collections.deque(maxlen=MAXLEN)
self.collelr = collections.deque(maxlen=MAXLEN)
self.collthl = collections.deque(maxlen=MAXLEN)
self.collcal = collections.deque(maxlen=MAXLEN)
self.collthr = collections.deque(maxlen=MAXLEN)
self.collcar = collections.deque(maxlen=MAXLEN)
#N=2 or 4, Wn = 0.01-0.1. 0<Wn<1
self.B, self.A = signal.butter(N, WN, output=OUTPUT)
def __init__(self, id, env, enable_legs=True, segway_sim=True,
base_frame='/map',
kin_frame='/head/skel_depth_frame2'):
assert id != ''
self.id = id
self.enabled = True
self.enable_legs = enable_legs
self.base_frame = base_frame
self.refsys = kin_frame
self.env = env
_,self.body = humanpy.initialize(sim=True, user_id=id, env=env)
self.body.Enable(False)
with env:
self.initialDOFvalues = self.body.GetDOFValues()
self.currentDOFvalues = self.body.GetDOFValues()
self.starting_angle = dict()
self.last_updated = rospy.get_rostime().secs
#!/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()
#!/usr/bin/env python
import humanpy
import numpy
import IPython
from prpy.rave import add_object
if __name__ == "__main__":
env, robot = humanpy.initialize(attach_viewer=True)
with env:
fuze = add_object(env, 'fuze_bottle', 'objects/fuze_bottle.kinbody.xml')
glass = add_object(env, 'plastic_glass', 'objects/plastic_glass.kinbody.xml')
fuze_pose = numpy.eye(4)
glass_pose = numpy.eye(4)
fuze_pose[0:3, 3] = [ 0.8, -0.3, 0.4 ]
glass_pose[0:3, 3] = [ 0.9, 0, 0.4 ]
fuze.SetTransform(fuze_pose)
glass.SetTransform(glass_pose)
fuze.Enable(True)
glass.Enable(True)
robotLocation = numpy.array([[ 0. , 0. , 1. , 0.4],
[ 1. , 0. , 0. , -0.1],
[ 0. , 1. , 0. , 0.3],
[ 0. , 0. , 0. , 1. ]])
robot.SetTransform(robotLocation)
robot.right_arm.SetActive()
#robot.right_arm.Grasp(fuze)
raw_input("press enter to quit!")
