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()
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()
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()
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()
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()
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()
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()
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()
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()
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()
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()
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()
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()
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()
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()
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()
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")
"""
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., )