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