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 
Exemple #3
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()
Exemple #4
0
#!/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!")