Esempio n. 1
0
class Ur(AbstractMobileRobot):
    initPosition = [0,0,0,0,0,0,0,0,0,0,0,0]
    def specifySpecialLinks(self):
        if len(self.SpecialLinks) == len(self.SpecialNames):
            for i in range(0,len(self.SpecialLinks)):
                self.dynamic.addJointMapping(self.SpecialLinks[i], self.SpecialNames[i])
        else:
            print 'No Special joints added : SpecialLinks.size != SpecialJoints.size'
    
    def __init__(self, name, device = None, tracer = None):
        AbstractMobileRobot.__init__ (self, name, tracer)
        # add operational points
        #self.OperationalPoints.append('waist')
        self.OperationalPoints.append('base_joint')
        self.OperationalPoints.append('shoulder_pan_joint')
        self.OperationalPoints.append('wrist_2_joint')
        self.OperationalPoints.append('wrist_3_joint')
        self.OperationalPoints.append('wrist_1_joint')
        self.OperationalPoints.append('shoulder_lift_joint')
        self.OperationalPoints.append('elbow_joint')
        # device and dynamic model assignment
        self.device = device
        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
        # load model
        self.dynamic.loadFromParameterServer()
        self.dimension = self.dynamic.getDimension()
        self.initPosition = (0.,) * self.dimension
        # initialize ur robot
        self.initializeRobot()
Esempio n. 2
0
    def __init__(self, name, device=None, tracer=None):
        AbstractHumanoidRobot.__init__(self, name, tracer)
        self.device = device
        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
        self.specifySpecialLinks()
        #Note: the param 'robot_description' should be defined before the next instruction
        self.dynamic.loadFromParameterServer()
        self.dimension = self.dynamic.getDimension()
        self.halfSitting = (0., ) * self.dimension
        lst = list(self.halfSitting)
        lst[24] = -0.33
        lst[26] = -0.47
        lst[39] = -0.33
        lst[41] = -0.47
        self.halfSitting = tuple(lst)

        # correct the initialization of the dynamic.
        self.dynamic.velocity.value = self.dimension * (0., )
        self.dynamic.acceleration.value = self.dimension * (0., )
        self.dynamic.ffposition.unplug()
        self.dynamic.ffvelocity.unplug()
        self.dynamic.ffacceleration.unplug()
        self.dynamic.setProperty('ComputeBackwardDynamics', 'true')
        self.dynamic.setProperty('ComputeAccelerationCoM', 'true')

        self.initializeRobot()
Esempio n. 3
0
class Ur(AbstractHumanoidRobot):
    """
    This class instanciates a Ur5 robot.
    """
    OperationalPoints = ['waist', 'wrist_3_joint']

    tracedSignals = {
        'dynamic': ["com", "position", "velocity", "acceleration"],
        'device': ['control', 'state']
    }

    def initializeUrRobot(self):
        """
        initialize ur robot
        """
        if not self.dynamic:
            raise RunTimeError("robots models have to be initialized first")
        if not self.device:
            self.device = RobotSimu(self.name + '_device')
        plug(self.device.state, self.dynamic.position)
        self.dynamic.velocity.value = self.dimension * (0., )
        self.dynamic.acceleration.value = self.dimension * (0., )
        self.initializeOpPoints(self.dynamic)

    def __init__(self, name, device=None, tracer=None):
        AbstractHumanoidRobot.__init__(self, name, tracer)
        self.device = device
        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
        #self.specifySpecialLinks()
        self.dynamic.loadFromParameterServer()
        self.dimension = self.dynamic.getDimension()
        self.initializeUrRobot()
Esempio n. 4
0
class Ur(AbstractMobileRobot):
    initPosition = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

    def specifySpecialLinks(self):
        if len(self.SpecialLinks) == len(self.SpecialNames):
            for i in range(0, len(self.SpecialLinks)):
                self.dynamic.addJointMapping(self.SpecialLinks[i],
                                             self.SpecialNames[i])
        else:
            print 'No Special joints added : SpecialLinks.size != SpecialJoints.size'

    def __init__(self, name, device=None, tracer=None):
        AbstractMobileRobot.__init__(self, name, tracer)
        # add operational points
        #self.OperationalPoints.append('waist')
        self.OperationalPoints.append('base_joint')
        self.OperationalPoints.append('shoulder_pan_joint')
        self.OperationalPoints.append('wrist_2_joint')
        self.OperationalPoints.append('wrist_3_joint')
        self.OperationalPoints.append('wrist_1_joint')
        self.OperationalPoints.append('shoulder_lift_joint')
        self.OperationalPoints.append('elbow_joint')
        # device and dynamic model assignment
        self.device = device
        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
        # load model
        self.dynamic.loadFromParameterServer()
        self.dimension = self.dynamic.getDimension()
        self.initPosition = (0., ) * self.dimension
        # initialize ur robot
        self.initializeRobot()
Esempio n. 5
0
class youbot(AbstractMobileRobot):
    """
    This class instanciates a Ur5 robot.
    """

    tracedSignals = {
        'dynamic': ["com", "position", "velocity", "acceleration"],
        'device': ['control', 'state']
        }
        
    initPosition = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
    def specifySpecialLinks(self):
        if len(self.SpecialLinks) == len(self.SpecialNames):
            for i in range(0,len(self.SpecialLinks)):
                self.dynamic.addJointMapping(self.SpecialLinks[i], self.SpecialNames[i])
        else:
            print 'No Special joints added : SpecialLinks.size != SpecialJoints.size'

    def __init__(self, name, device = None, tracer = None):
        AbstractMobileRobot.__init__ (self, name, tracer)
        # add operational points
        self.OperationalPoints.append('waist')
        self.OperationalPoints.append('arm_joint_5')
        self.OperationalPoints.append('base_joint')

        # device and dynamic model assignment
        self.device = device
        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
        # load model
        self.dynamic.loadFromParameterServer()
        self.dimension = self.dynamic.getDimension()
        self.initPosition = (0.,) * self.dimension
        # initialize ur robot
        self.initializeRobot()
Esempio n. 6
0
class youbot(AbstractMobileRobot):
    """
    This class instanciates a Ur5 robot.
    """

    tracedSignals = {
        'dynamic': ["com", "position", "velocity", "acceleration"],
        'device': ['control', 'state']
        }
        
    initPosition = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
    def specifySpecialLinks(self):
        if len(self.SpecialLinks) == len(self.SpecialNames):
            for i in range(0,len(self.SpecialLinks)):
                self.dynamic.addJointMapping(self.SpecialLinks[i], self.SpecialNames[i])
        else:
            print 'No Special joints added : SpecialLinks.size != SpecialJoints.size'

    def __init__(self, name, device = None, tracer = None):
        AbstractMobileRobot.__init__ (self, name, tracer)
        # add operational points
        self.OperationalPoints.append('waist')
        self.OperationalPoints.append('arm_joint_5')
        self.OperationalPoints.append('base_joint')

        # device and dynamic model assignment
        self.device = device
        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
        # load model
        self.dynamic.loadFromParameterServer()
        self.dimension = self.dynamic.getDimension()
        self.initPosition = (0.,) * self.dimension
        # initialize ur robot
        self.initializeRobot()
Esempio n. 7
0
class Ur(AbstractHumanoidRobot):
    """
    This class instanciates a Ur5 robot.
    """
    OperationalPoints = ['waist','wrist_3_joint']
    
    tracedSignals = {
        'dynamic': ["com", "position", "velocity", "acceleration"],
        'device': ['control', 'state']
        }
        
    def initializeUrRobot(self):
        """
        initialize ur robot
        """
        if not self.dynamic:
            raise RunTimeError("robots models have to be initialized first")
        if not self.device:
            self.device = RobotSimu(self.name + '_device')
        plug(self.device.state, self.dynamic.position)
        self.dynamic.velocity.value = self.dimension*(0.,)
        self.dynamic.acceleration.value = self.dimension*(0.,)
        self.initializeOpPoints(self.dynamic)

    def __init__(self, name, device = None, tracer = None):
        AbstractHumanoidRobot.__init__ (self, name, tracer)
        self.device = device
        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
        #self.specifySpecialLinks()
        self.dynamic.loadFromParameterServer()
        self.dimension = self.dynamic.getDimension()
        self.initializeUrRobot()
Esempio n. 8
0
 def __init__(self, name, device=None, tracer=None):
     AbstractHumanoidRobot.__init__(self, name, tracer)
     self.device = device
     self.dynamic = RosRobotModel("{0}_dynamic".format(name))
     #self.specifySpecialLinks()
     self.dynamic.loadFromParameterServer()
     self.dimension = self.dynamic.getDimension()
     self.initializeUrRobot()
Esempio n. 9
0
class Pr2(AbstractMobileRobot):
    """
    This class instanciates a Ur5 robot.
    """

    tracedSignals = {
        'dynamic': ["com", "position", "velocity", "acceleration"],
        'device': ['control', 'state']
        }
        
    #initPosition = [0,0,0,0,0,0,0.011,0.011,-0.1, 0.023, 0.12,0,0]
    
    def __init__(self, name, device = None, tracer = None):
        AbstractMobileRobot.__init__ (self, name, tracer)
        # add operational points
        ip = (0.,0.,0.,0.,0.,0.,0.011,0.0,0.0,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.)
        self.OperationalPoints.append('base_joint')
        self.OperationalPoints.append('r_shoulder_pan_joint')
        self.OperationalPoints.append('r_shoulder_lift_joint')
        self.OperationalPoints.append('l_shoulder_pan_joint')
        self.OperationalPoints.append('l_shoulder_lift_joint')
        self.OperationalPoints.append('r_wrist_roll_joint')
        self.OperationalPoints.append('l_wrist_roll_joint')
        self.OperationalPoints.append('r_wrist_flex_joint')
        self.OperationalPoints.append('l_wrist_flex_joint')
        self.OperationalPoints.append('torso_lift_joint')
        self.OperationalPoints.append('l_upper_arm_roll_joint')
        self.OperationalPoints.append('r_upper_arm_roll_joint')
        self.OperationalPoints.append('l_forearm_roll_joint')
        self.OperationalPoints.append('r_forearm_roll_joint')
        #self.OperationalPoints.append('l_forearm_flex_joint')
        #self.OperationalPoints.append('r_forearm_flex_joint')


#        self.OperationalPoints.append('arm_joint_2')
#        self.OperationalPoints.append('arm_joint_3')      
#        self.OperationalPoints.append('arm_joint_4')
#        self.OperationalPoints.append('arm_joint_5')

        # device and dynamic model assignment
        self.device = device
        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
        # load model
        self.dynamic.loadFromParameterServer()
        self.dimension = self.dynamic.getDimension()
        self.initPosition = ip
        #self.initPosition = (0.,) * self.dimension
        # initialize ur robot
        self.initializeRobot()        
Esempio n. 10
0
    def __init__(self, name, device = None, tracer = None):
        AbstractHumanoidRobot.__init__ (self, name, tracer)
        self.device = device
        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
        self.specifySpecialLinks()
        #Note: the param 'robot_description' should be defined before the next instruction
        self.dynamic.loadFromParameterServer()
        self.dimension = self.dynamic.getDimension()
        self.halfSitting = (0.,) * self.dimension
        lst = list(self.halfSitting)
        lst[24] = -0.33
        lst[26] = -0.47
        lst[39] = -0.33
        lst[41] = -0.47
        self.halfSitting = tuple(lst)

        # correct the initialization of the dynamic.
        self.dynamic.velocity.value = self.dimension*(0.,)
        self.dynamic.acceleration.value = self.dimension*(0.,)
        self.dynamic.ffposition.unplug()
        self.dynamic.ffvelocity.unplug()
        self.dynamic.ffacceleration.unplug()
        self.dynamic.setProperty('ComputeBackwardDynamics','true')
        self.dynamic.setProperty('ComputeAccelerationCoM','true')

        self.initializeRobot()
Esempio n. 11
0
    def __init__(self, name, device=None, tracer=None):
        AbstractMobileRobot.__init__(self, name, tracer)
        # add operational points
        self.OperationalPoints.append('waist')
        self.OperationalPoints.append('arm_joint_5')
        self.OperationalPoints.append('base_joint')

        # device and dynamic model assignment
        self.device = device
        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
        # load model
        self.dynamic.loadFromParameterServer()
        self.dimension = self.dynamic.getDimension()
        self.initPosition = (0., ) * self.dimension
        # initialize ur robot
        self.initializeRobot()
Esempio n. 12
0
 def __init__(self, name, device = None, tracer = None):
     AbstractHumanoidRobot.__init__ (self, name, tracer)
     self.device = device
     self.dynamic = RosRobotModel("{0}_dynamic".format(name))
     #self.specifySpecialLinks()
     self.dynamic.loadFromParameterServer()
     self.dimension = self.dynamic.getDimension()
     self.initializeUrRobot()
Esempio n. 13
0
class Pr2(AbstractHumanoidRobot):
    """
    This class instanciates a Pr2 robot.
    """

    OperationalPoints = [
        'right-wrist', 'left-wrist', 'waist', 'gaze', 'chest', 'left-ankle'
    ]

    jointMap = {}
    jointMap['BODY'] = 'base_link'
    jointMap['l_wrist'] = 'l_gripper_palm_link'
    jointMap['r_wrist'] = 'r_gripper_palm_link'
    jointMap['l_gripper'] = 'l_gripper_tool_frame'
    jointMap['r_gripper'] = 'r_gripper_tool_frame'
    jointMap['gaze'] = 'double_stereo_link'
    jointMap['torso'] = 'torso_lift_link'
    jointMap['l_ankle'] = 'base_link'  # TODO?

    tracedSignals = {
        'dynamic': ["com", "position", "velocity", "acceleration"],
        'device': ['control', 'state']
    }

    def specifySpecialLinks(self):
        for i in self.jointMap:
            self.dynamic.addJointMapping(i, self.jointMap[i])

    def __init__(self, name, device=None, tracer=None):
        AbstractHumanoidRobot.__init__(self, name, tracer)
        self.device = device
        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
        self.specifySpecialLinks()
        #Note: the param 'robot_description' should be defined before the next instruction
        self.dynamic.loadFromParameterServer()
        self.dimension = self.dynamic.getDimension()
        self.halfSitting = (0., ) * self.dimension
        if self.device:
            self.device.control.value = self.dimension * (0., )
        lst = list(self.halfSitting)
        lst[24] = -0.33
        lst[26] = -0.47
        lst[39] = -0.33
        lst[41] = -0.47
        self.halfSitting = tuple(lst)

        # correct the initialization of the dynamic.
        self.dynamic.velocity.value = self.dimension * (0., )
        self.dynamic.acceleration.value = self.dimension * (0., )
        self.dynamic.ffposition.unplug()
        self.dynamic.ffvelocity.unplug()
        self.dynamic.ffacceleration.unplug()
        self.dynamic.setProperty('ComputeBackwardDynamics', 'true')
        self.dynamic.setProperty('ComputeAccelerationCoM', 'true')

        self.initializeRobot()
Esempio n. 14
0
class Pr2(AbstractHumanoidRobot):
    """
    This class instanciates a Pr2 robot.
    """

    OperationalPoints = ['right-wrist','left-wrist','waist','gaze','chest','left-ankle']

    jointMap={}
    jointMap['BODY']      = 'base_link'
    jointMap['l_wrist']   = 'l_gripper_palm_link'
    jointMap['r_wrist']   = 'r_gripper_palm_link'
    jointMap['l_gripper'] = 'l_gripper_tool_frame'
    jointMap['r_gripper'] = 'r_gripper_tool_frame'
    jointMap['gaze']      = 'double_stereo_link'
    jointMap['torso']     = 'torso_lift_link'
    jointMap['l_ankle']   = 'base_link' # TODO?

    tracedSignals = {
        'dynamic': ["com", "position", "velocity", "acceleration"],
        'device': ['control', 'state']
        }
        
    def specifySpecialLinks(self):
        for i in self.jointMap:
            self.dynamic.addJointMapping(i, self.jointMap[i])

    def __init__(self, name, device = None, tracer = None):
        AbstractHumanoidRobot.__init__ (self, name, tracer)
        self.device = device
        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
        self.specifySpecialLinks()
        #Note: the param 'robot_description' should be defined before the next instruction
        self.dynamic.loadFromParameterServer()
        self.dimension = self.dynamic.getDimension()
        self.halfSitting = (0.,) * self.dimension
        if self.device:
            self.device.control.value = self.dimension * (0.,)
        lst = list(self.halfSitting)
        lst[24] = -0.33
        lst[26] = -0.47
        lst[39] = -0.33
        lst[41] = -0.47
        self.halfSitting = tuple(lst)

        # correct the initialization of the dynamic.
        self.dynamic.velocity.value = self.dimension*(0.,)
        self.dynamic.acceleration.value = self.dimension*(0.,)
        self.dynamic.ffposition.unplug()
        self.dynamic.ffvelocity.unplug()
        self.dynamic.ffacceleration.unplug()
        self.dynamic.setProperty('ComputeBackwardDynamics','true')
        self.dynamic.setProperty('ComputeAccelerationCoM','true')

        self.initializeRobot()
Esempio n. 15
0
    def __init__(self, name, device = None, tracer = None):
        AbstractMobileRobot.__init__ (self, name, tracer)
        # add operational points
        self.OperationalPoints.append('waist')
        self.OperationalPoints.append('arm_joint_5')
        self.OperationalPoints.append('base_joint')

        # device and dynamic model assignment
        self.device = device
        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
        # load model
        self.dynamic.loadFromParameterServer()
        self.dimension = self.dynamic.getDimension()
        self.initPosition = (0.,) * self.dimension
        # initialize ur robot
        self.initializeRobot()
Esempio n. 16
0
    def __init__(self, name, device = None, tracer = None):
        AbstractMobileRobot.__init__ (self, name, tracer)
        # add operational points
        ip = (0.,0.,0.,0.,0.,0.,0.011,0.0,0.0,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.)
        self.OperationalPoints.append('base_joint')
        self.OperationalPoints.append('r_shoulder_pan_joint')
        self.OperationalPoints.append('r_shoulder_lift_joint')
        self.OperationalPoints.append('l_shoulder_pan_joint')
        self.OperationalPoints.append('l_shoulder_lift_joint')
        self.OperationalPoints.append('r_wrist_roll_joint')
        self.OperationalPoints.append('l_wrist_roll_joint')
        self.OperationalPoints.append('r_wrist_flex_joint')
        self.OperationalPoints.append('l_wrist_flex_joint')
        self.OperationalPoints.append('torso_lift_joint')
        self.OperationalPoints.append('l_upper_arm_roll_joint')
        self.OperationalPoints.append('r_upper_arm_roll_joint')
        self.OperationalPoints.append('l_forearm_roll_joint')
        self.OperationalPoints.append('r_forearm_roll_joint')
        #self.OperationalPoints.append('l_forearm_flex_joint')
        #self.OperationalPoints.append('r_forearm_flex_joint')


#        self.OperationalPoints.append('arm_joint_2')
#        self.OperationalPoints.append('arm_joint_3')      
#        self.OperationalPoints.append('arm_joint_4')
#        self.OperationalPoints.append('arm_joint_5')

        # device and dynamic model assignment
        self.device = device
        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
        # load model
        self.dynamic.loadFromParameterServer()
        self.dimension = self.dynamic.getDimension()
        self.initPosition = ip
        #self.initPosition = (0.,) * self.dimension
        # initialize ur robot
        self.initializeRobot()        
Esempio n. 17
0
def addTrace(device, trace, entityName, signalName, autoRecompute = True):
    """
    Add a signal to a tracer and recompute it automatically if necessary.
    """
    signal = '{0}.{1}'.format(entityName, signalName)
    filename = '{0}-{1}'.format(entityName, signalName)
    trace.add(signal, filename)
    if autoRecompute:
        device.after.addSignal(signal)

I4 =   ((1.,0,0,0),
	(0,1.,0,0),
	(0,0,1.,0),
	(0,0,0,1.),)
	
model = RosRobotModel ('ur5_dynamic')
device = RobotSimu ('ur5_device')

rospy.init_node('fake')
model.loadUrdf ("file:///local/ngiftsun/devel/ros/catkin_ws/src/ur_description/urdf/ur5_robot.urdf")

dimension = model.getDimension ()
device.resize (dimension)

plug (device.state, model.position)
# Set velocity and acceleration to 0
model.velocity.value = dimension * (0.,)
model.acceleration.value = dimension * (0.,)

# Create taks for the base
model.createOpPoint ("base", "waist")
Esempio n. 18
0
    """
    signal = '{0}.{1}'.format(entityName, signalName)
    filename = '{0}-{1}'.format(entityName, signalName)
    trace.add(signal, filename)
    if autoRecompute:
        device.after.addSignal(signal)


I4 = (
    (1., 0, 0, 0),
    (0, 1., 0, 0),
    (0, 0, 1., 0),
    (0, 0, 0, 1.),
)

model = RosRobotModel('ur5_dynamic')
device = RobotSimu('ur5_device')

rospy.init_node('fake')
model.loadUrdf(
    "file:///local/ngiftsun/devel/ros/catkin_ws/src/ur_description/urdf/ur5_robot.urdf"
)

dimension = model.getDimension()
device.resize(dimension)

plug(device.state, model.position)
# Set velocity and acceleration to 0
model.velocity.value = dimension * (0., )
model.acceleration.value = dimension * (0., )