class Pr2(AbstractRobot): """ This class instanciates a Pr2 robot. """ OperationalPoints = ['right-wrist','left-wrist','waist','gaze','chest','left-ankle'] SpecialLinks = ['BODY', 'l_wrist', 'r_wrist', 'l_gripper', 'r_gripper', 'gaze','torso','l_ankle'] SpecialNames = ['base_link', 'l_wrist_roll_link', 'r_wrist_roll_link', 'l_gripper_palm_link', 'r_gripper_palm_link', 'head_tilt_link','torso_lift_link','base_link'] tracedSignals = { 'dynamic': ["com", "position", "velocity", "acceleration"], 'device': ['control', 'state'] } 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): AbstractRobot.__init__ (self, name, tracer) self.device = device self.dynamic = RosSotRobotModel("{0}_dynamic".format(name)) self.specifySpecialLinks() self.dynamic.loadFromParameterServer() self.dimension = self.dynamic.getDimension() self.halfSitting = (0.,) * self.dimension self.initializeRobot()
class RobotBuilder(AbstractHumanoidRobot): """ This class instanciates a robot. """ #OperationalPoints = ['right-wrist','left-wrist','waist','right-ankle','left-ankle'] #OperationalPoints = ['right-wrist','left-wrist','waist','gaze'] tracedSignals = { 'dynamic': ["com", "zmp", "position", "velocity", "acceleration"], 'device': ['zmp', 'control', 'state'] } def __init__(self, name, device=None, tracer=None, ns='sot_controller'): AbstractHumanoidRobot.__init__ (self, name, tracer) self.device = device self.dynamic = RosSotRobotModel("{0}_dynamic".format(name)) self.dynamic.setNamespace(ns) self.dynamic.loadFromParameterServer() self.halfSitting = self.dynamic.curConf() self.dimension = self.dynamic.getDimension() self.initializeRobot()
class RobotBuilder(AbstractHumanoidRobot): """ This class instanciates a robot. """ #OperationalPoints = ['right-wrist','left-wrist','waist','right-ankle','left-ankle'] #OperationalPoints = ['right-wrist','left-wrist','waist','gaze'] tracedSignals = { 'dynamic': ["com", "zmp", "position", "velocity", "acceleration"], 'device': ['zmp', 'control', 'state'] } def __init__(self, name, device=None, tracer=None, ns='sot_controller'): AbstractHumanoidRobot.__init__(self, name, tracer) self.device = device self.dynamic = RosSotRobotModel("{0}_dynamic".format(name)) self.dynamic.setNamespace(ns) self.dynamic.loadFromParameterServer() self.halfSitting = self.dynamic.curConf() self.dimension = self.dynamic.getDimension() self.initializeRobot()
pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015") pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.0 3.0") pg.parseCmd(":UpperBodyMotionParameters 0.0 -0.5 0.0") pg.parseCmd(":comheight 0.814") pg.parseCmd(":SetAlgoForZmpTrajectory Morisawa") plug(robot.dynamic.position,pg.position) plug(robot.dynamic.com,pg.com) pg.motorcontrol.value = robot.dynamic.getDimension()*(0,) pg.zmppreviouscontroller.value = (0,0,0) pg.initState() geom = RosSotRobotModel('geom') geom.setNamespace('sot_controller') geom.loadFromParameterServer() geom.createOpPoint('rf','right-ankle') geom.createOpPoint('lf','left-ankle') plug(robot.dynamic.position,geom.position) geom.ffposition.value = 6*(0,) geom.velocity.value = robot.dynamic.getDimension()*(0,) geom.acceleration.value = robot.dynamic.getDimension()*(0,) comRef = Selector('comRef',['vector','ref',robot.dynamic.com,pg.comref]) plug(pg.inprocess,comRef.selec) selecSupportFoot = Selector('selecSupportFoot',['matrixHomo','pg_H_sf',pg.rightfootref,pg.leftfootref],['matrixHomo','wa_H_sf',geom.rf,geom.lf]) plug(pg.SupportFoot,selecSupportFoot.selec) sf_H_wa = Inverse_of_matrixHomo('sf_H_wa')
pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015") pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.0 3.0") pg.parseCmd(":UpperBodyMotionParameters 0.0 -0.5 0.0") pg.parseCmd(":comheight 0.814") pg.parseCmd(":SetAlgoForZmpTrajectory Morisawa") plug(robot.dynamic.position, pg.position) plug(robot.dynamic.com, pg.com) pg.motorcontrol.value = robot.dynamic.getDimension() * (0, ) pg.zmppreviouscontroller.value = (0, 0, 0) pg.initState() geom = RosSotRobotModel('geom') geom.setNamespace('sot_controller') geom.loadFromParameterServer() geom.createOpPoint('rf', 'right-ankle') geom.createOpPoint('lf', 'left-ankle') plug(robot.dynamic.position, geom.position) geom.ffposition.value = 6 * (0, ) geom.velocity.value = robot.dynamic.getDimension() * (0, ) geom.acceleration.value = robot.dynamic.getDimension() * (0, ) comRef = Selector('comRef', ['vector', 'ref', robot.dynamic.com, pg.comref]) plug(pg.inprocess, comRef.selec) selecSupportFoot = Selector( 'selecSupportFoot', ['matrixHomo', 'pg_H_sf', pg.rightfootref, pg.leftfootref], ['matrixHomo', 'wa_H_sf', geom.rf, geom.lf])