Basic RT components and utilities  315.7.0
Public Member Functions | Public Attributes | Static Public Attributes | List of all members
python.hrpsys_config.HrpsysConfigurator Class Reference

Public Member Functions

def connectComps
 Connect components(plugins) More...
 
def activateComps
 Activate components(plugins) More...
 
def deactivateComps
 Deactivate components(plugins) More...
 
def createComp
 Create RTC component (plugins) More...
 
def createComps
 Create components(plugins) in getRTCList() More...
 
def deleteComp
 Delete RTC component (plugins) More...
 
def deleteComps
 Delete components(plugins) in getRTCInstanceList() More...
 
def findComp
 Find component(plugin) More...
 
def findComps
 Check if all components in getRTCList() are created. More...
 
def getRTCList
 Get list of available STABLE components. More...
 
def getRTCListUnstable
 Get list of available components including stable and unstable. More...
 
def getJointAngleControllerList
 Get list of controller list that need to control joint angles. More...
 
def getRTCInstanceList
 Get list of RTC Instance. More...
 
def parseUrl
 
def getBodyInfo
 Get bodyInfo. More...
 
def getSensors
 Get list of sensors. More...
 
def getForceSensorNames
 Get list of force sensor names. More...
 
def connectLoggerPort
 Connect port to logger. More...
 
def setupLogger
 Setup logging function. More...
 
def waitForRTCManager
 Wait for RTC manager. More...
 
def waitForRobotHardware
 Wait for RobotHardware is exists and activated. More...
 
def checkSimulationMode
 Check if this is running as simulation. More...
 
def waitForRTCManagerAndRoboHardware
 Wait for both RTC Manager (waitForRTCManager()) and RobotHardware (waitForRobotHardware()) More...
 
def findModelLoader
 Try to find ModelLoader. More...
 
def waitForModelLoader
 Wait for ModelLoader. More...
 
def setSelfGroups
 Set to the hrpsys.SequencePlayer the groups of links and joints that are statically defined as member variables (Groups) within this class. More...
 
def goActual
 Adjust commanded values to the angles in the physical state by calling StateHolder::goActual. More...
 
def setJointAngle
 Set angle to the given joint. More...
 
def setJointAngles
 Set all joint angles. More...
 
def setJointAnglesOfGroup
 Set the joint angles to aim. More...
 
def setJointAnglesSequence
 Set all joint angles. More...
 
def setJointAnglesSequenceOfGroup
 Set all joint angles. More...
 
def loadPattern
 Load a pattern file that is created offline. More...
 
def waitInterpolation
 Lets SequencePlayer wait until the movement currently happening to finish. More...
 
def waitInterpolationOfGroup
 Lets SequencePlayer wait until the movement currently happening to finish. More...
 
def getJointAngles
 Returns the commanded joint angle values. More...
 
def getCurrentPose
 Returns the current physical pose of the specified joint. More...
 
def getCurrentPosition
 Returns the current physical position of the specified joint. More...
 
def getCurrentRotation
 Returns the current physical rotation of the specified joint. More...
 
def getCurrentRPY
 Returns the current physical rotation in RPY of the specified joint. More...
 
def getReferencePose
 Returns the current commanded pose of the specified joint. More...
 
def getReferencePosition
 Returns the current commanded position of the specified joint. More...
 
def getReferenceRotation
 Returns the current commanded rotation of the specified joint. More...
 
def getReferenceRPY
 Returns the current commanded rotation in RPY of the specified joint. More...
 
def setTargetPose
 Move the end-effector to the given absolute pose. More...
 
def setTargetPoseRelative
 Move the end-effector's location relative to its current pose. More...
 
def clear
 Clears the Sequencer's current operation. More...
 
def clearOfGroup
 Clears the Sequencer's current operation for joint groups. More...
 
def saveLog
 Save log to the given file name. More...
 
def clearLog
 Clear logger's buffer. More...
 
def setMaxLogLength
 Set logger's buffer. More...
 
def lengthDigitalInput
 Returns the length of digital input port. More...
 
def lengthDigitalOutput
 Returns the length of digital output port. More...
 
def writeDigitalOutput
 Using writeDigitalOutputWithMask is recommended for the less data transport. More...
 
def writeDigitalOutputWithMask
 Both dout and mask are lists with length of 32. More...
 
def readDigitalInput
 Read data from Digital Input. More...
 
def readDigitalOutput
 Read data from Digital Output. More...
 
def getActualState
 Get actual states of the robot that includes current reference joint angles and joint torques. More...
 
def isCalibDone
 Check whether joints have been calibrated. More...
 
def isServoOn
 Check whether servo control has been turned on. More...
 
def flat2Groups
 Convert list of angles into list of joint list for each groups. More...
 
def servoOn
 Turn on servos. More...
 
def servoOff
 Turn off servos. More...
 
def checkEncoders
 Run the encoder checking sequence for specified joints, run goActual and turn on servos. More...
 
def removeForceSensorOffset
 remove force sensor offset More...
 
def playPattern
 Play motion pattern using a given trajectory that is represented by a list of joint angles, rpy, zmp and time. More...
 
def playPatternOfGroup
 Play motion pattern using a given trajectory that is represented by a list of joint angles. More...
 
def setSensorCalibrationJointAngles
 Set joint angles for sensor calibration. More...
 
def calibrateInertiaSensor
 Calibrate inertia sensor. More...
 
def calibrateInertiaSensorWithDialog
 Calibrate inertia sensor with dialog and setting calibration pose. More...
 
def startAutoBalancer
 Start AutoBalancer mode. More...
 
def stopAutoBalancer
 Stop AutoBalancer mode. More...
 
def startStabilizer
 Start Stabilzier mode. More...
 
def stopStabilizer
 Stop Stabilzier mode. More...
 
def startImpedance_315_4
 start impedance mode More...
 
def stopImpedance_315_4
 stop impedance mode More...
 
def startImpedance
 
def stopImpedance
 
def startDefaultUnstableControllers
 Start default unstable RTCs controller mode. More...
 
def stopDefaultUnstableControllers
 Stop default unstable RTCs controller mode. More...
 
def setFootSteps
 setFootSteps More...
 
def setFootStepsWithParam
 setFootSteps More...
 
def init
 Calls init from its superclass, which tries to connect RTCManager, looks for ModelLoader, and starts necessary RTC components. More...
 
def __init__
 

Public Attributes

 configurator_name
 

Static Public Attributes

 rh = None
 
 rh_svc = None
 
 ep_svc = None
 
 rh_version = None
 
 seq = None
 
 seq_svc = None
 
 seq_version = None
 
 sh = None
 
 sh_svc = None
 
 sh_version = None
 
 sc = None
 
 sc_svc = None
 
 sc_version = None
 
 fk = None
 
 fk_svc = None
 
 fk_version = None
 
 tf = None
 
 kf = None
 
 vs = None
 
 rmfo = None
 
 ic = None
 
 abc = None
 
 st = None
 
 tf_version = None
 
 kf_version = None
 
 vs_version = None
 
 rmfo_version = None
 
 ic_version = None
 
 abc_version = None
 
 st_version = None
 
 es = None
 
 es_svc = None
 
 es_version = None
 
 hes = None
 
 hes_svc = None
 
 hes_version = None
 
 co = None
 
 co_svc = None
 
 co_version = None
 
 gc = None
 
 gc_svc = None
 
 gc_version = None
 
 el = None
 
 el_svc = None
 
 el_version = None
 
 te = None
 
 te_svc = None
 
 te_version = None
 
 tl = None
 
 tl_svc = None
 
 tl_version = None
 
 tc = None
 
 tc_svc = None
 
 tc_version = None
 
 log = None
 
 log_svc = None
 
 log_version = None
 
 log_use_owned_ec = False
 
 ms = None
 
 hgc = None
 
 pdc = None
 
 simulation_mode = None
 
 sensors = None
 
list Groups = []
 
 hrpsys_version = None
 
 kinematics_only_mode = False
 

Constructor & Destructor Documentation

def python.hrpsys_config.HrpsysConfigurator.__init__ (   self,
  cname = "[hrpsys.py] " 
)

Member Function Documentation

def python.hrpsys_config.HrpsysConfigurator.activateComps (   self)
def python.hrpsys_config.HrpsysConfigurator.calibrateInertiaSensor (   self)
def python.hrpsys_config.HrpsysConfigurator.calibrateInertiaSensorWithDialog (   self)
def python.hrpsys_config.HrpsysConfigurator.checkEncoders (   self,
  jname = 'all',
  option = '' 
)

Run the encoder checking sequence for specified joints, run goActual and turn on servos.

Parameters
jnamestr: The value 'all' works iteratively for all servos.
optionstr: Possible values are follows (w/o double quote):\ "-overwrite": Overwrite calibration value.

References python.hrpsys_config.HrpsysConfigurator.configurator_name, python.hrpsys_config.HrpsysConfigurator.goActual(), python.hrpsys_config.HrpsysConfigurator.isCalibDone(), python.hrpsys_config.HrpsysConfigurator.isServoOn(), and python.hrpsys_config.HrpsysConfigurator.sc_svc.

def python.hrpsys_config.HrpsysConfigurator.checkSimulationMode (   self)
def python.hrpsys_config.HrpsysConfigurator.clear (   self)
def python.hrpsys_config.HrpsysConfigurator.clearLog (   self)

Clear logger's buffer.

def python.hrpsys_config.HrpsysConfigurator.clearOfGroup (   self,
  gname,
  tm = 0.0 
)

Clears the Sequencer's current operation for joint groups.

def python.hrpsys_config.HrpsysConfigurator.connectComps (   self)
def python.hrpsys_config.HrpsysConfigurator.connectLoggerPort (   self,
  artc,
  sen_name,
  log_name = None 
)

Connect port to logger.

Parameters
artcobject: object of component that contains sen_name port
sen_namestr: name of port for logging
log_namestr: name of port in logger

References python.hrpsys_config.HrpsysConfigurator.configurator_name, and python.rtm.connectPorts().

Referenced by python.hrpsys_config.HrpsysConfigurator.setupLogger().

def python.hrpsys_config.HrpsysConfigurator.createComp (   self,
  compName,
  instanceName 
)

Create RTC component (plugins)

Parameters
instanceNamestr: name of instance, choose one of https://github.com/fkanehiro/hrpsys-base/tree/master/rtc
comNamestr: name of component that to be created.

References python.hrpsys_config.HrpsysConfigurator.configurator_name, and python.rtm.narrow().

def python.hrpsys_config.HrpsysConfigurator.createComps (   self)
def python.hrpsys_config.HrpsysConfigurator.deactivateComps (   self)
def python.hrpsys_config.HrpsysConfigurator.deleteComp (   self,
  compName 
)

Delete RTC component (plugins)

Parameters
compNamestr: name of component that to be deleted

Referenced by python.hrpsys_config.HrpsysConfigurator.deleteComps().

def python.hrpsys_config.HrpsysConfigurator.deleteComps (   self)
def python.hrpsys_config.HrpsysConfigurator.findComp (   self,
  compName,
  instanceName,
  max_timeout_count = 10,
  verbose = True 
)

Find component(plugin)

Parameters
compNamestr: component name
instanceNamestr: instance name int: max timeout in seconds

References python.hrpsys_config.HrpsysConfigurator.configurator_name, and python.rtm.narrow().

def python.hrpsys_config.HrpsysConfigurator.findComps (   self,
  max_timeout_count = 10,
  verbose = True 
)
def python.hrpsys_config.HrpsysConfigurator.findModelLoader (   self)

Try to find ModelLoader.

Referenced by python.hrpsys_config.HrpsysConfigurator.waitForModelLoader().

def python.hrpsys_config.HrpsysConfigurator.flat2Groups (   self,
  flatList 
)

Convert list of angles into list of joint list for each groups.

Parameters
flatListlist: single dimension list with its length of 15
Returns
list of list: 2-dimensional list of Groups.

References python.hrpsys_config.HrpsysConfigurator.Groups.

def python.hrpsys_config.HrpsysConfigurator.getActualState (   self)

Get actual states of the robot that includes current reference joint angles and joint torques.

Returns
: This returns actual states of the robot, which is defined in RobotHardware.idl (https://github.com/fkanehiro/hrpsys-base/blob/master/idl/RobotHardwareService.idl#L33)
    /**
     * @brief status of the robot
     */
    struct RobotState
    {
      DblSequence               angle;  ///< current joint angles[rad]
      DblSequence               command;///< reference joint angles[rad]
      DblSequence               torque; ///< joint torques[Nm]
      /**
       * @brief servo statuses(32bit+extra states)
       *
       * 0: calib status ( 1 => done )\n
       * 1: servo status ( 1 => on )\n
       * 2: power status ( 1 => supplied )\n
       * 3-18: servo alarms (see @ref iob.h)\n
       * 19-23: unused
       * 24-31: driver temperature (deg)
       */
      LongSequenceSequence              servoState;
      sequence<DblSequence6>    force;    ///< forces[N] and torques[Nm]
      sequence<DblSequence3>    rateGyro; ///< angular velocities[rad/s]
      sequence<DblSequence3>    accel;    ///< accelerations[m/(s^2)]
      double                    voltage;  ///< voltage of power supply[V]
      double                    current;  ///< current[A]
    };

Referenced by python.hrpsys_config.HrpsysConfigurator.isServoOn().

def python.hrpsys_config.HrpsysConfigurator.getBodyInfo (   self,
  url 
)
def python.hrpsys_config.HrpsysConfigurator.getCurrentPose (   self,
  lname = None,
  frame_name = None 
)

Returns the current physical pose of the specified joint.

cf. getReferencePose that returns commanded value.

eg.

     IN: robot.getCurrentPose('LARM_JOINT5')
     OUT: [-0.0017702356144599085,
      0.00019034630541264752,
      -0.9999984150158207,
      0.32556275164378523,
      0.00012155879975329215,
      0.9999999745367515,
       0.0001901314142046251,
       0.18236394191140365,
       0.9999984257434246,
       -0.00012122202968358842,
       -0.001770258707652326,
       0.07462472659364472,
       0.0,
       0.0,
       0.0,
       1.0]

lname: str

Parameters
lname,:Name of the link.
frame_namestr: set reference frame name (from 315.2.5) : list of float
Returns
: Rotational matrix and the position of the given joint in 1-dimensional list, that is:
 [a11, a12, a13, x,
  a21, a22, a23, y,
  a31, a32, a33, z,
   0,   0,   0,  1]

References python.hrpsys_config.HrpsysConfigurator.fk_version, python.hrpsys_config.HrpsysConfigurator.getCurrentPose(), and python.hrpsys_config.HrpsysConfigurator.Groups.

Referenced by python.hrpsys_config.HrpsysConfigurator.getCurrentPose(), python.hrpsys_config.HrpsysConfigurator.getCurrentPosition(), python.hrpsys_config.HrpsysConfigurator.getCurrentRotation(), and python.hrpsys_config.HrpsysConfigurator.setTargetPoseRelative().

def python.hrpsys_config.HrpsysConfigurator.getCurrentPosition (   self,
  lname = None,
  frame_name = None 
)

Returns the current physical position of the specified joint.

cf. getReferencePosition that returns commanded value.

eg.

    robot.getCurrentPosition('LARM_JOINT5')
    [0.325, 0.182, 0.074]

lname: str

Parameters
lname,:Name of the link.
frame_namestr: set reference frame name (from 315.2.5) : list of float
Returns
: List of x, y, z positions about the specified joint.

References python.hrpsys_config.HrpsysConfigurator.getCurrentPose(), python.hrpsys_config.HrpsysConfigurator.getCurrentPosition(), and python.hrpsys_config.HrpsysConfigurator.Groups.

Referenced by python.hrpsys_config.HrpsysConfigurator.getCurrentPosition().

def python.hrpsys_config.HrpsysConfigurator.getCurrentRotation (   self,
  lname,
  frame_name = None 
)

Returns the current physical rotation of the specified joint.

cf. getReferenceRotation that returns commanded value.

lname: str

Parameters
lname,:Name of the link.
frame_namestr: set reference frame name (from 315.2.5) : list of float
Returns
: Rotational matrix of the given joint in 2-dimensional list, that is:
 [[a11, a12, a13],
  [a21, a22, a23],
  [a31, a32, a33]]

References python.hrpsys_config.HrpsysConfigurator.getCurrentPose(), python.hrpsys_config.HrpsysConfigurator.getCurrentRotation(), and python.hrpsys_config.HrpsysConfigurator.Groups.

Referenced by python.hrpsys_config.HrpsysConfigurator.getCurrentRotation(), and python.hrpsys_config.HrpsysConfigurator.getCurrentRPY().

def python.hrpsys_config.HrpsysConfigurator.getCurrentRPY (   self,
  lname,
  frame_name = None 
)

Returns the current physical rotation in RPY of the specified joint.

cf. getReferenceRPY that returns commanded value.

lname: str

Parameters
lname,:Name of the link.
frame_namestr: set reference frame name (from 315.2.5) : list of float
Returns
: List of orientation in rpy form about the specified joint.

References python.hrpsys_config.euler_from_matrix(), python.hrpsys_config.HrpsysConfigurator.getCurrentRotation(), python.hrpsys_config.HrpsysConfigurator.getCurrentRPY(), and python.hrpsys_config.HrpsysConfigurator.Groups.

Referenced by python.hrpsys_config.HrpsysConfigurator.getCurrentRPY().

def python.hrpsys_config.HrpsysConfigurator.getForceSensorNames (   self)

Get list of force sensor names.

Returns existence force sensors and virtual force sensors. self.sensors and virtual force sensors are assumed.

References python.hrpsys_config.HrpsysConfigurator.sensors, and python.hrpsys_config.HrpsysConfigurator.vs.

Referenced by python.hrpsys_config.HrpsysConfigurator.connectComps().

def python.hrpsys_config.HrpsysConfigurator.getJointAngleControllerList (   self)
def python.hrpsys_config.HrpsysConfigurator.getJointAngles (   self)

Returns the commanded joint angle values.

See Also
: HrpsysConfigurator.getJointAngles

Note that it's not the physical state of the robot's joints, which can be obtained by getActualState().angle.

Returns
List of float: List of angles (degree) of all joints, in the order defined in the member variable 'Groups' (eg. chest, head1, head2, ..).
def python.hrpsys_config.HrpsysConfigurator.getReferencePose (   self,
  lname,
  frame_name = None 
)

Returns the current commanded pose of the specified joint.

cf. getCurrentPose that returns physical pose.

lname: str

Parameters
lname,:Name of the link.
frame_namestr: set reference frame name (from 315.2.5) : list of float
Returns
: Rotational matrix and the position of the given joint in 1-dimensional list, that is:
 [a11, a12, a13, x,
  a21, a22, a23, y,
  a31, a32, a33, z,
   0,   0,   0,  1]

References python.hrpsys_config.HrpsysConfigurator.fk_version, python.hrpsys_config.HrpsysConfigurator.getReferencePose(), and python.hrpsys_config.HrpsysConfigurator.Groups.

Referenced by python.hrpsys_config.HrpsysConfigurator.getReferencePose(), python.hrpsys_config.HrpsysConfigurator.getReferencePosition(), and python.hrpsys_config.HrpsysConfigurator.getReferenceRotation().

def python.hrpsys_config.HrpsysConfigurator.getReferencePosition (   self,
  lname,
  frame_name = None 
)

Returns the current commanded position of the specified joint.

cf. getCurrentPosition that returns physical value.

lname: str

Parameters
lname,:Name of the link.
frame_namestr: set reference frame name (from 315.2.5) : list of float
Returns
: List of angles (degree) of all joints, in the order defined in the member variable 'Groups' (eg. chest, head1, head2, ..).

References python.hrpsys_config.HrpsysConfigurator.getReferencePose(), python.hrpsys_config.HrpsysConfigurator.getReferencePosition(), and python.hrpsys_config.HrpsysConfigurator.Groups.

Referenced by python.hrpsys_config.HrpsysConfigurator.getReferencePosition().

def python.hrpsys_config.HrpsysConfigurator.getReferenceRotation (   self,
  lname,
  frame_name = None 
)

Returns the current commanded rotation of the specified joint.

cf. getCurrentRotation that returns physical value.

lname: str

Parameters
lname,:Name of the link.
frame_namestr: set reference frame name (from 315.2.5) : list of float
Returns
: Rotational matrix of the given joint in 2-dimensional list, that is:
 [[a11, a12, a13],
  [a21, a22, a23],
  [a31, a32, a33]]

References python.hrpsys_config.HrpsysConfigurator.getReferencePose(), and python.hrpsys_config.HrpsysConfigurator.Groups.

Referenced by python.hrpsys_config.HrpsysConfigurator.getReferenceRPY().

def python.hrpsys_config.HrpsysConfigurator.getReferenceRPY (   self,
  lname,
  frame_name = None 
)

Returns the current commanded rotation in RPY of the specified joint.

cf. getCurrentRPY that returns physical value.

lname: str

Parameters
lname,:Name of the link.
frame_namestr: set reference frame name (from 315.2.5) : list of float
Returns
: List of orientation in rpy form about the specified joint.

References python.hrpsys_config.euler_from_matrix(), python.hrpsys_config.HrpsysConfigurator.getReferenceRotation(), python.hrpsys_config.HrpsysConfigurator.getReferenceRPY(), and python.hrpsys_config.HrpsysConfigurator.Groups.

Referenced by python.hrpsys_config.HrpsysConfigurator.getReferenceRPY().

def python.hrpsys_config.HrpsysConfigurator.getRTCInstanceList (   self,
  verbose = True 
)
def python.hrpsys_config.HrpsysConfigurator.getRTCList (   self)

Get list of available STABLE components.

Returns
list of list: List of available components. Each element consists of a list of abbreviated and full names of the component.

Referenced by python.hrpsys_config.HrpsysConfigurator.createComps(), python.hrpsys_config.HrpsysConfigurator.findComps(), and python.hrpsys_config.HrpsysConfigurator.getRTCInstanceList().

def python.hrpsys_config.HrpsysConfigurator.getRTCListUnstable (   self)

Get list of available components including stable and unstable.

Returns
list of list: List of available unstable components. Each element consists of a list of abbreviated and full names of the component.
def python.hrpsys_config.HrpsysConfigurator.getSensors (   self,
  url 
)

Get list of sensors.

Parameters
urlstr: model file url

References python.hrpsys_config.HrpsysConfigurator.getBodyInfo().

Referenced by python.hrpsys_config.HrpsysConfigurator.init().

def python.hrpsys_config.HrpsysConfigurator.goActual (   self)

Adjust commanded values to the angles in the physical state by calling StateHolder::goActual.

This needs to be run BEFORE servos are turned on.

Referenced by python.hrpsys_config.HrpsysConfigurator.checkEncoders(), and python.hrpsys_config.HrpsysConfigurator.servoOn().

def python.hrpsys_config.HrpsysConfigurator.init (   self,
  robotname = "Robot",
  url = "" 
)
def python.hrpsys_config.HrpsysConfigurator.isCalibDone (   self)

Check whether joints have been calibrated.

Returns
bool: True if all joints are calibrated

References python.hrpsys_config.HrpsysConfigurator.simulation_mode.

Referenced by python.hrpsys_config.HrpsysConfigurator.checkEncoders(), and python.hrpsys_config.HrpsysConfigurator.servoOn().

def python.hrpsys_config.HrpsysConfigurator.isServoOn (   self,
  jname = 'any' 
)

Check whether servo control has been turned on.

Parameters
jnamestr: Name of a link (that can be obtained by "hiro.Groups" as lists of groups). When jname = 'all' or 'any' => If all joints are servoOn, return True, otherwise, return False. When jname = 'some' => If some joint is servoOn, return True, otherwise return False.
Returns
bool: True if servo is on

References python.hrpsys_config.HrpsysConfigurator.configurator_name, python.hrpsys_config.HrpsysConfigurator.getActualState(), and python.hrpsys_config.HrpsysConfigurator.simulation_mode.

Referenced by python.hrpsys_config.HrpsysConfigurator.checkEncoders(), python.hrpsys_config.HrpsysConfigurator.servoOff(), and python.hrpsys_config.HrpsysConfigurator.servoOn().

def python.hrpsys_config.HrpsysConfigurator.lengthDigitalInput (   self)

Returns the length of digital input port.

def python.hrpsys_config.HrpsysConfigurator.lengthDigitalOutput (   self)
def python.hrpsys_config.HrpsysConfigurator.loadPattern (   self,
  fname,
  tm 
)

Load a pattern file that is created offline.

Format of the pattern file:

  • example format:
      t0 j0 j1 j2...jn
      t1 j0 j1 j2...jn
      :
      tn j0 j1 j2...jn
    
  • Delimmitted by space
  • Each line consists of an action.
  • Time between each action is defined by tn+1 - tn
    • The time taken for the 1st line is defined by the arg tm.
Parameters
fnamestr: Name of the pattern file.
tmfloat: - The time to take for the 1st line.
Returns
: List of 2 oct(string) values.

References python.hrpsys_config.HrpsysConfigurator.parseUrl().

def python.hrpsys_config.HrpsysConfigurator.parseUrl (   self,
  url 
)
def python.hrpsys_config.HrpsysConfigurator.playPattern (   self,
  jointangles,
  rpy,
  zmp,
  tm 
)

Play motion pattern using a given trajectory that is represented by a list of joint angles, rpy, zmp and time.

Parameters
jointangleslist of list of float: The whole list represents a trajectory. Each element of the 1st degree in the list consists of the joint angles.
rpylist of float: Orientation in rpy.
zmplist of float: TODO: description
tmfloat: Time to complete the task.
Returns
bool:
def python.hrpsys_config.HrpsysConfigurator.playPatternOfGroup (   self,
  gname,
  jointangles,
  tm 
)

Play motion pattern using a given trajectory that is represented by a list of joint angles.

Parameters
gnamestr: Name of the joint group.
jointangleslist of list of float: The whole list represents a trajectory. Each element of the 1st degree in the list consists of the joint angles. To illustrate:

[[a0-0, a0-1,...,a0-n], # a)ngle. 1st path in trajectory [a1-0, a1-1,...,a1-n], # 2nd path in the trajectory. : [am-0, am-1,...,am-n]] # mth path in the trajectory

Parameters
tmfloat: Time to complete the task.
Returns
bool:
def python.hrpsys_config.HrpsysConfigurator.readDigitalInput (   self)

Read data from Digital Input.

See Also
: HrpsysConfigurator.readDigitalInput

Digital input consits of 14 bits. The last 2 bits are lacking and compensated, so that the last 4 bits are 0x4 instead of 0x1.

Author
Hajime Saito (@emijah)
Returns
list of int: List of the values (2 octtets) in digital input register. Range: 0 or 1.

#TODO: Catch AttributeError that occurs when RobotHardware not found.

Typically with simulator, erro msg might look like this;

'NoneType' object has no attribute 'readDigitalInput'

References python.hrpsys_config.HrpsysConfigurator.simulation_mode.

def python.hrpsys_config.HrpsysConfigurator.readDigitalOutput (   self)

Read data from Digital Output.

Digital input consits of 14 bits. The last 2 bits are lacking and compensated, so that the last 4 bits are 0x4 instead of 0x1.

#TODO: Catch AttributeError that occurs when RobotHardware not found.

Typically with simulator, erro msg might look like this;

'NoneType' object has no attribute 'readDigitaloutput'

Author
Hajime Saito (@emijah)
Returns
list of int: List of the values in digital input register. Range: 0 or 1.
def python.hrpsys_config.HrpsysConfigurator.removeForceSensorOffset (   self)

remove force sensor offset

def python.hrpsys_config.HrpsysConfigurator.saveLog (   self,
  fname = 'sample' 
)

Save log to the given file name.

Parameters
fnamestr: name of the file

References python.hrpsys_config.HrpsysConfigurator.configurator_name.

def python.hrpsys_config.HrpsysConfigurator.servoOff (   self,
  jname = 'all',
  wait = True 
)

Turn off servos.

Parameters
jnamestr: The value 'all' works iteratively for all servos.
waitbool: Wait for user's confirmation via GUI
Returns
int: 1 = all arm servo off. 2 = all servo on arms and hands off. -1 = Something wrong happened.

References python.hrpsys_config.HrpsysConfigurator.configurator_name, python.hrpsys_config.HrpsysConfigurator.isServoOn(), python.hrpsys_config.HrpsysConfigurator.sc_svc, and python.hrpsys_config.HrpsysConfigurator.simulation_mode.

def python.hrpsys_config.HrpsysConfigurator.servoOn (   self,
  jname = 'all',
  destroy = 1,
  tm = 3 
)

Turn on servos.

Joints need to be calibrated (otherwise error returns).

Parameters
jnamestr: The value 'all' works iteratively for all servos.
destroyint: Not used.
tmfloat: Second to complete.
Returns
int: 1 or -1 indicating success or failure, respectively.

References python.hrpsys_config.HrpsysConfigurator.configurator_name, python.hrpsys_config.HrpsysConfigurator.goActual(), python.hrpsys_config.HrpsysConfigurator.isCalibDone(), and python.hrpsys_config.HrpsysConfigurator.isServoOn().

def python.hrpsys_config.HrpsysConfigurator.setFootSteps (   self,
  footstep,
  overwrite_fs_idx = 0 
)

setFootSteps

Parameters
footstep: FootstepSequence.
overwrite_fs_idx: Index to be overwritten. overwrite_fs_idx is used only in walking.
def python.hrpsys_config.HrpsysConfigurator.setFootStepsWithParam (   self,
  footstep,
  stepparams,
  overwrite_fs_idx = 0 
)

setFootSteps

Parameters
footstep: FootstepSequence.
stepparams: StepParamSeuqnce.
overwrite_fs_idx: Index to be overwritten. overwrite_fs_idx is used only in walking.
def python.hrpsys_config.HrpsysConfigurator.setJointAngle (   self,
  jname,
  angle,
  tm 
)

Set angle to the given joint.

NOTE-1: It's known that this method does not do anything after
some group operation is done.
TODO: at least need to warn users.
NOTE-2: While this method does not check angle value range,
any joints could emit position limit over error, which has not yet
been thrown by hrpsys so that there's no way to catch on this python client. 
Worthwhile opening an enhancement ticket at designated issue tracker.
Parameters
jnamestr: name of joint
anglefloat: In degree.
tmfloat: Time to complete.
def python.hrpsys_config.HrpsysConfigurator.setJointAngles (   self,
  angles,
  tm 
)

Set all joint angles.

NOTE: While this method does not check angle value range,
      any joints could emit position limit over error, which has not yet
      been thrown by hrpsys so that there's no way to catch on this python client. 
      Worthwhile opening an enhancement ticket at designated issue tracker.
Parameters
angleslist of float: In degree.
tmfloat: Time to complete.

Referenced by python.hrpsys_config.HrpsysConfigurator.setSensorCalibrationJointAngles().

def python.hrpsys_config.HrpsysConfigurator.setJointAnglesOfGroup (   self,
  gname,
  pose,
  tm,
  wait = True 
)

Set the joint angles to aim.

By default it waits interpolation to be over.

NOTE: While this method does not check angle value range,
      any joints could emit position limit over error, which has not yet
      been thrown by hrpsys so that there's no way to catch on this python client. 
      Worthwhile opening an enhancement ticket at designated issue tracker.
Parameters
gnamestr: Name of the joint group.
poselist of float: list of positions and orientations
tmfloat: Time to complete.
waitbool: If true, all other subsequent commands wait until the movement commanded by this method call finishes.

References python.hrpsys_config.HrpsysConfigurator.waitInterpolationOfGroup().

def python.hrpsys_config.HrpsysConfigurator.setJointAnglesSequence (   self,
  angless,
  tms 
)

Set all joint angles.

NOTE: While this method does not check angle value range,
      any joints could emit position limit over error, which has not yet
      been thrown by hrpsys so that there's no way to catch on this python client. 
      Worthwhile opening an enhancement ticket at designated issue tracker.
Parameters
sequenceangles list of float: In degree.
tmsequence of float: Time to complete, In Second
def python.hrpsys_config.HrpsysConfigurator.setJointAnglesSequenceOfGroup (   self,
  gname,
  angless,
  tms 
)

Set all joint angles.

NOTE: While this method does not check angle value range,
      any joints could emit position limit over error, which has not yet
      been thrown by hrpsys so that there's no way to catch on this python client. 
      Worthwhile opening an enhancement ticket at designated issue tracker.
Parameters
gnamestr: Name of the joint group.
sequenceangles list of float: In degree.
tmsequence of float: Time to complete, In Second
def python.hrpsys_config.HrpsysConfigurator.setMaxLogLength (   self,
  length 
)

Set logger's buffer.

Parameters
lengthint: length of log, if the system runs at 500hz and you want to store 2min, set 2*60*500.
def python.hrpsys_config.HrpsysConfigurator.setSelfGroups (   self)

Set to the hrpsys.SequencePlayer the groups of links and joints that are statically defined as member variables (Groups) within this class.

References python.hrpsys_config.HrpsysConfigurator.Groups.

Referenced by python.hrpsys_config.HrpsysConfigurator.init().

def python.hrpsys_config.HrpsysConfigurator.setSensorCalibrationJointAngles (   self)

Set joint angles for sensor calibration.

Please override joint angles to avoid self collision.

References python.hrpsys_config.HrpsysConfigurator.setJointAngles(), and python.hrpsys_config.HrpsysConfigurator.waitInterpolation().

Referenced by python.hrpsys_config.HrpsysConfigurator.calibrateInertiaSensorWithDialog().

def python.hrpsys_config.HrpsysConfigurator.setTargetPose (   self,
  gname,
  pos,
  rpy,
  tm,
  frame_name = None 
)

Move the end-effector to the given absolute pose.

All d* arguments are in meter.

Parameters
gnamestr: Name of the joint group.
poslist of float: In meter.
rpylist of float: In radian.
tmfloat: Second to complete the command.
frame_namestr: Name of the frame that this particular command references to.
Returns
bool: False if unreachable.

References python.hrpsys_config.HrpsysConfigurator.setTargetPoseRelative().

Referenced by python.hrpsys_config.HrpsysConfigurator.setTargetPoseRelative().

def python.hrpsys_config.HrpsysConfigurator.setTargetPoseRelative (   self,
  gname,
  eename,
  dx = 0,
  dy = 0,
  dz = 0,
  dr = 0,
  dp = 0,
  dw = 0,
  tm = 10,
  wait = True 
)

Move the end-effector's location relative to its current pose.

For d*, distance arguments are in meter while rotations are in degree.

Example usage: The following moves RARM_JOINT5 joint 0.1mm forward within 0.1sec.

    robot.setTargetPoseRelative('rarm', 'RARM_JOINT5', dx=0.0001, tm=0.1)
Parameters
gnamestr: Name of the joint group.
eenamestr: Name of the link.
dxfloat: In meter.
dyfloat: In meter.
dzfloat: In meter.
drfloat: In radian.
dpfloat: In radian.
dwfloat: In radian.
tmfloat: Second to complete the command.
waitbool: If true, all other subsequent commands wait until the movement commanded by this method call finishes.
Returns
bool: False if unreachable.

References python.hrpsys_config.euler_from_matrix(), python.hrpsys_config.euler_matrix(), python.hrpsys_config.HrpsysConfigurator.getCurrentPose(), python.hrpsys_config.HrpsysConfigurator.setTargetPose(), and python.hrpsys_config.HrpsysConfigurator.waitInterpolationOfGroup().

Referenced by python.hrpsys_config.HrpsysConfigurator.setTargetPose().

def python.hrpsys_config.HrpsysConfigurator.setupLogger (   self,
  maxLength = 4000 
)
def python.hrpsys_config.HrpsysConfigurator.startAutoBalancer (   self,
  limbs = ["rleg",
  lleg 
)

Start AutoBalancer mode.

Parameters
limbslist of end-effector name to control. rleg and lleg by default.

Referenced by python.hrpsys_config.HrpsysConfigurator.startDefaultUnstableControllers().

def python.hrpsys_config.HrpsysConfigurator.startDefaultUnstableControllers (   self,
  ic_limbs = ["rarm",
  larm,
  abc_limbs = ["rleg",
  lleg 
)

Start default unstable RTCs controller mode.

Currently Stabilzier, AutoBalancer, and ImpedanceController are started.

References python.hrpsys_config.HrpsysConfigurator.startAutoBalancer(), and python.hrpsys_config.HrpsysConfigurator.startStabilizer().

def python.hrpsys_config.HrpsysConfigurator.startImpedance (   self,
  arm,
  kwargs 
)
def python.hrpsys_config.HrpsysConfigurator.startImpedance_315_4 (   self,
  arm,
  M_p = 100.0,
  D_p = 100.0,
  K_p = 100.0,
  M_r = 100.0,
  D_r = 2000.0,
  K_r = 2000.0,
  force_gain = [1,
  moment_gain = [0,
  sr_gain = 1.0,
  avoid_gain = 0.0,
  reference_gain = 0.0,
  manipulability_limit = 0.1,
  controller_mode = None,
  ik_optional_weight_vector = None 
)

start impedance mode

arm: str name of artm to be controlled, this must be initialized using setSelfGroups()

References python.hrpsys_config.HrpsysConfigurator.configurator_name.

Referenced by python.hrpsys_config.HrpsysConfigurator.startImpedance(), and python.hrpsys_config.HrpsysConfigurator.stopStabilizer().

def python.hrpsys_config.HrpsysConfigurator.startStabilizer (   self)
def python.hrpsys_config.HrpsysConfigurator.stopAutoBalancer (   self)
def python.hrpsys_config.HrpsysConfigurator.stopDefaultUnstableControllers (   self,
  ic_limbs = ["rarm",
  larm 
)

Stop default unstable RTCs controller mode.

Currently Stabilzier, AutoBalancer, and ImpedanceController are stopped.

References python.hrpsys_config.HrpsysConfigurator.stopAutoBalancer(), and python.hrpsys_config.HrpsysConfigurator.stopStabilizer().

def python.hrpsys_config.HrpsysConfigurator.stopImpedance (   self,
  arm 
)
def python.hrpsys_config.HrpsysConfigurator.stopImpedance_315_4 (   self,
  arm 
)

stop impedance mode

Referenced by python.hrpsys_config.HrpsysConfigurator.stopImpedance().

def python.hrpsys_config.HrpsysConfigurator.stopStabilizer (   self)
def python.hrpsys_config.HrpsysConfigurator.waitForModelLoader (   self)
def python.hrpsys_config.HrpsysConfigurator.waitForRobotHardware (   self,
  robotname = "Robot" 
)

Wait for RobotHardware is exists and activated.

Parameters
robotnamestr: name of RobotHardware component.

References python.hrpsys_config.HrpsysConfigurator.configurator_name, python.hrpsys_config.HrpsysConfigurator.ms, and python.hrpsys_config.HrpsysConfigurator.rh.

Referenced by python.hrpsys_config.HrpsysConfigurator.waitForRTCManagerAndRoboHardware().

def python.hrpsys_config.HrpsysConfigurator.waitForRTCManager (   self,
  managerhost = nshost 
)

Wait for RTC manager.

Parameters
managerhoststr: name of host computer that manager is running

References python.hrpsys_config.HrpsysConfigurator.configurator_name, and python.hrpsys_config.HrpsysConfigurator.ms.

Referenced by python.hrpsys_config.HrpsysConfigurator.waitForRTCManagerAndRoboHardware().

def python.hrpsys_config.HrpsysConfigurator.waitForRTCManagerAndRoboHardware (   self,
  robotname = "Robot",
  managerhost = nshost 
)

Wait for both RTC Manager (waitForRTCManager()) and RobotHardware (waitForRobotHardware())

Parameters
managerhoststr: name of host computer that manager is running
robotnamestr: name of RobotHardware component.

References python.hrpsys_config.HrpsysConfigurator.checkSimulationMode(), python.hrpsys_config.HrpsysConfigurator.waitForRobotHardware(), and python.hrpsys_config.HrpsysConfigurator.waitForRTCManager().

Referenced by python.hrpsys_config.HrpsysConfigurator.init().

def python.hrpsys_config.HrpsysConfigurator.waitInterpolation (   self)

Lets SequencePlayer wait until the movement currently happening to finish.

Referenced by python.hrpsys_config.HrpsysConfigurator.setSensorCalibrationJointAngles().

def python.hrpsys_config.HrpsysConfigurator.waitInterpolationOfGroup (   self,
  gname 
)

Lets SequencePlayer wait until the movement currently happening to finish.

See Also
: SequencePlayer.waitInterpolationOfGroup
: http://wiki.ros.org/joint_trajectory_action. This method corresponds to JointTrajectoryGoal in ROS.
Parameters
gnamestr: Name of the joint group.

Referenced by python.hrpsys_config.HrpsysConfigurator.setJointAnglesOfGroup(), and python.hrpsys_config.HrpsysConfigurator.setTargetPoseRelative().

def python.hrpsys_config.HrpsysConfigurator.writeDigitalOutput (   self,
  dout 
)

Using writeDigitalOutputWithMask is recommended for the less data transport.

Parameters
doutlist of int: List of bits, length of 32 bits where elements are 0 or 1.

What each element stands for depends on how the robot's imlemented. Consult the hardware manual.

Returns
bool: RobotHardware.writeDigitalOutput returns True if writable. False otherwise.

References python.hrpsys_config.HrpsysConfigurator.lengthDigitalOutput(), and python.hrpsys_config.HrpsysConfigurator.simulation_mode.

def python.hrpsys_config.HrpsysConfigurator.writeDigitalOutputWithMask (   self,
  dout,
  mask 
)

Both dout and mask are lists with length of 32.

Only the bit in dout that corresponds to the bits in mask that are flagged as 1 will be evaluated.

Example:

 Case-1. Only 18th bit will be evaluated as 1.
  dout [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
  mask [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

 Case-2. Only 18th bit will be evaluated as 0.
  dout [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]
  mask [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

 Case-3. None will be evaluated since there's no flagged bit in mask.
  dout [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
  mask [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]
Parameters
doutlist of int: List of bits, length of 32 bits where elements are 0 or 1.
masklist of int: List of masking bits, length of 32 bits where elements are 0 or 1.
Returns
bool: RobotHardware.writeDigitalOutput returns True if writable. False otherwise.

References python.hrpsys_config.HrpsysConfigurator.lengthDigitalOutput(), and python.hrpsys_config.HrpsysConfigurator.simulation_mode.

Member Data Documentation

python.hrpsys_config.HrpsysConfigurator.abc = None
static
python.hrpsys_config.HrpsysConfigurator.abc_version = None
static
python.hrpsys_config.HrpsysConfigurator.co = None
static
python.hrpsys_config.HrpsysConfigurator.co_svc = None
static
python.hrpsys_config.HrpsysConfigurator.co_version = None
static
python.hrpsys_config.HrpsysConfigurator.configurator_name
python.hrpsys_config.HrpsysConfigurator.el = None
static
python.hrpsys_config.HrpsysConfigurator.el_svc = None
static
python.hrpsys_config.HrpsysConfigurator.el_version = None
static
python.hrpsys_config.HrpsysConfigurator.ep_svc = None
static
python.hrpsys_config.HrpsysConfigurator.es = None
static
python.hrpsys_config.HrpsysConfigurator.es_svc = None
static
python.hrpsys_config.HrpsysConfigurator.es_version = None
static
python.hrpsys_config.HrpsysConfigurator.fk = None
static
python.hrpsys_config.HrpsysConfigurator.fk_svc = None
static
python.hrpsys_config.HrpsysConfigurator.fk_version = None
static
python.hrpsys_config.HrpsysConfigurator.gc = None
static
python.hrpsys_config.HrpsysConfigurator.gc_svc = None
static
python.hrpsys_config.HrpsysConfigurator.gc_version = None
static
list python.hrpsys_config.HrpsysConfigurator.Groups = []
static
python.hrpsys_config.HrpsysConfigurator.hes = None
static
python.hrpsys_config.HrpsysConfigurator.hes_svc = None
static
python.hrpsys_config.HrpsysConfigurator.hes_version = None
static
python.hrpsys_config.HrpsysConfigurator.hgc = None
static
python.hrpsys_config.HrpsysConfigurator.hrpsys_version = None
static
python.hrpsys_config.HrpsysConfigurator.ic = None
static
python.hrpsys_config.HrpsysConfigurator.ic_version = None
static
python.hrpsys_config.HrpsysConfigurator.kf = None
static
python.hrpsys_config.HrpsysConfigurator.kf_version = None
static
python.hrpsys_config.HrpsysConfigurator.kinematics_only_mode = False
static
python.hrpsys_config.HrpsysConfigurator.log = None
static
python.hrpsys_config.HrpsysConfigurator.log_svc = None
static
python.hrpsys_config.HrpsysConfigurator.log_use_owned_ec = False
static
python.hrpsys_config.HrpsysConfigurator.log_version = None
static
python.hrpsys_config.HrpsysConfigurator.ms = None
static
python.hrpsys_config.HrpsysConfigurator.pdc = None
static
python.hrpsys_config.HrpsysConfigurator.rh = None
static
python.hrpsys_config.HrpsysConfigurator.rh_svc = None
static
python.hrpsys_config.HrpsysConfigurator.rh_version = None
static
python.hrpsys_config.HrpsysConfigurator.rmfo = None
static
python.hrpsys_config.HrpsysConfigurator.rmfo_version = None
static
python.hrpsys_config.HrpsysConfigurator.sc = None
static
python.hrpsys_config.HrpsysConfigurator.sc_svc = None
static
python.hrpsys_config.HrpsysConfigurator.sc_version = None
static
python.hrpsys_config.HrpsysConfigurator.sensors = None
static
python.hrpsys_config.HrpsysConfigurator.seq = None
static
python.hrpsys_config.HrpsysConfigurator.seq_svc = None
static
python.hrpsys_config.HrpsysConfigurator.seq_version = None
static
python.hrpsys_config.HrpsysConfigurator.sh = None
static
python.hrpsys_config.HrpsysConfigurator.sh_svc = None
static
python.hrpsys_config.HrpsysConfigurator.sh_version = None
static
python.hrpsys_config.HrpsysConfigurator.simulation_mode = None
static
python.hrpsys_config.HrpsysConfigurator.st = None
static
python.hrpsys_config.HrpsysConfigurator.st_version = None
static
python.hrpsys_config.HrpsysConfigurator.tc = None
static
python.hrpsys_config.HrpsysConfigurator.tc_svc = None
static
python.hrpsys_config.HrpsysConfigurator.tc_version = None
static
python.hrpsys_config.HrpsysConfigurator.te = None
static
python.hrpsys_config.HrpsysConfigurator.te_svc = None
static
python.hrpsys_config.HrpsysConfigurator.te_version = None
static
python.hrpsys_config.HrpsysConfigurator.tf = None
static
python.hrpsys_config.HrpsysConfigurator.tf_version = None
static
python.hrpsys_config.HrpsysConfigurator.tl = None
static
python.hrpsys_config.HrpsysConfigurator.tl_svc = None
static
python.hrpsys_config.HrpsysConfigurator.tl_version = None
static
python.hrpsys_config.HrpsysConfigurator.vs = None
static
python.hrpsys_config.HrpsysConfigurator.vs_version = None
static

The documentation for this class was generated from the following file: