Basic RT components and utilities  315.15.0
Public Member Functions | Public Attributes | Static Public Attributes | List of all members
python.hrpsys_config.HrpsysConfigurator Class Reference
Inheritance diagram for python.hrpsys_config.HrpsysConfigurator:
Inheritance graph
[legend]
Collaboration diagram for python.hrpsys_config.HrpsysConfigurator:
Collaboration graph
[legend]

Public Member Functions

def connectComps (self)
 Connect components(plugins) More...
 
def activateComps (self)
 Activate components(plugins) More...
 
def deactivateComps (self)
 Deactivate components(plugins) More...
 
def createComp (self, compName, instanceName)
 Create RTC component (plugins) More...
 
def createComps (self)
 Create components(plugins) in getRTCList() More...
 
def deleteComp (self, compName)
 Delete RTC component (plugins) More...
 
def deleteComps (self)
 Delete components(plugins) in getRTCInstanceList() More...
 
def findComp (self, compName, instanceName, max_timeout_count=10, verbose=True)
 Find component(plugin) More...
 
def findComps (self, max_timeout_count=10, verbose=True)
 Check if all components in getRTCList() are created. More...
 
def getRTCList (self)
 Get list of available STABLE components. More...
 
def getRTCListUnstable (self)
 Get list of available components including stable and unstable. More...
 
def getJointAngleControllerList (self)
 Get list of controller list that need to control joint angles. More...
 
def getRTCInstanceList (self, verbose=True)
 Get list of RTC Instance. More...
 
def parseUrl (self, url)
 
def getBodyInfo (self, url)
 Get bodyInfo. More...
 
def getSensors (self, url)
 Get list of sensors. More...
 
def getForceSensorNames (self)
 Get list of force sensor names. More...
 
def connectLoggerPort (self, artc, sen_name, log_name=None)
 Connect port to logger. More...
 
def setupLogger (self, maxLength=4000)
 Setup logging function. More...
 
def waitForRTCManager (self, managerhost=nshost)
 Wait for RTC manager. More...
 
def waitForRobotHardware (self, robotname="Robot")
 Wait for RobotHardware is exists and activated. More...
 
def checkSimulationMode (self)
 Check if this is running as simulation. More...
 
def waitForRTCManagerAndRoboHardware (self, robotname="Robot", managerhost=nshost)
 
def waitForRTCManagerAndRobotHardware (self, robotname="Robot", managerhost=nshost)
 Wait for both RTC Manager (waitForRTCManager()) and RobotHardware (waitForRobotHardware()) More...
 
def findModelLoader (self)
 Try to find ModelLoader. More...
 
def waitForModelLoader (self)
 Wait for ModelLoader. More...
 
def setSelfGroups (self)
 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 (self)
 Adjust commanded values to the angles in the physical state by calling StateHolder::goActual. More...
 
def setJointAngle (self, jname, angle, tm)
 Set angle to the given joint. More...
 
def setJointAngles (self, angles, tm)
 Set all joint angles. More...
 
def setJointAnglesOfGroup (self, gname, pose, tm, wait=True)
 Set the joint angles to aim. More...
 
def setJointAnglesSequence (self, angless, tms)
 Set all joint angles. More...
 
def setJointAnglesSequenceOfGroup (self, gname, angless, tms)
 Set all joint angles. More...
 
def loadPattern (self, fname, tm)
 Load a pattern file that is created offline. More...
 
def waitInterpolation (self)
 Lets SequencePlayer wait until the movement currently happening to finish. More...
 
def waitInterpolationOfGroup (self, gname)
 Lets SequencePlayer wait until the movement currently happening to finish. More...
 
def setInterpolationMode (self, mode)
 Set interpolation mode. More...
 
def getJointAngles (self)
 Returns the commanded joint angle values. More...
 
def getCurrentPose (self, lname=None, frame_name=None)
 Returns the current physical pose of the specified joint in the joint space. More...
 
def getCurrentPosition (self, lname=None, frame_name=None)
 Returns the current physical position of the specified joint in Cartesian space. More...
 
def getCurrentRotation (self, lname, frame_name=None)
 Returns the current physical rotation of the specified joint. More...
 
def getCurrentRPY (self, lname, frame_name=None)
 Returns the current physical rotation in RPY of the specified joint. More...
 
def getReferencePose (self, lname, frame_name=None)
 Returns the current commanded pose of the specified joint in the joint space. More...
 
def getReferencePosition (self, lname, frame_name=None)
 Returns the current commanded position of the specified joint in Cartesian space. More...
 
def getReferenceRotation (self, lname, frame_name=None)
 Returns the current commanded rotation of the specified joint. More...
 
def getReferenceRPY (self, lname, frame_name=None)
 Returns the current commanded rotation in RPY of the specified joint. More...
 
def setTargetPose (self, gname, pos, rpy, tm, frame_name=None)
 Move the end-effector to the given absolute pose. More...
 
def 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. More...
 
def clear (self)
 Clears the Sequencer's current operation. More...
 
def clearOfGroup (self, gname, tm=0.0)
 Clears the Sequencer's current operation for joint groups. More...
 
def saveLog (self, fname='sample')
 Save log to the given file name. More...
 
def clearLog (self)
 Clear logger's buffer. More...
 
def setMaxLogLength (self, length)
 Set logger's buffer. More...
 
def lengthDigitalInput (self)
 Returns the length of digital input port. More...
 
def lengthDigitalOutput (self)
 Returns the length of digital output port. More...
 
def writeDigitalOutput (self, dout)
 Using writeDigitalOutputWithMask is recommended for the less data transport. More...
 
def writeDigitalOutputWithMask (self, dout, mask)
 Both dout and mask are lists with length of 32. More...
 
def readDigitalInput (self)
 Read data from Digital Input. More...
 
def readDigitalOutput (self)
 Read data from Digital Output. More...
 
def getActualState (self)
 Get actual states of the robot that includes current reference joint angles and joint torques. More...
 
def isCalibDone (self)
 Check whether joints have been calibrated. More...
 
def isServoOn (self, jname='any')
 Check whether servo control has been turned on. More...
 
def flat2Groups (self, flatList)
 Convert list of angles into list of joint list for each groups. More...
 
def servoOn (self, jname='all', destroy=1, tm=3)
 Turn on servos. More...
 
def servoOff (self, jname='all', wait=True)
 Turn off servos. More...
 
def checkEncoders (self, jname='all', option='')
 Run the encoder checking sequence for specified joints, run goActual and turn on servos. More...
 
def removeForceSensorOffset (self)
 remove force sensor offset More...
 
def 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. More...
 
def playPatternOfGroup (self, gname, jointangles, tm)
 Play motion pattern using a set of given trajectories that are represented by lists of joint angles. More...
 
def setSensorCalibrationJointAngles (self)
 Set joint angles for sensor calibration. More...
 
def calibrateInertiaSensor (self)
 Calibrate inertia sensor. More...
 
def calibrateInertiaSensorWithDialog (self)
 Calibrate inertia sensor with dialog and setting calibration pose. More...
 
def startAutoBalancer (self, limbs=None)
 Start AutoBalancer mode. More...
 
def stopAutoBalancer (self)
 Stop AutoBalancer mode. More...
 
def startStabilizer (self)
 Start Stabilzier mode. More...
 
def stopStabilizer (self)
 Stop Stabilzier mode. More...
 
def 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 More...
 
def stopImpedance_315_4 (self, arm)
 stop impedance mode More...
 
def startImpedance (self, arm, kwargs)
 Enable the ImpedanceController RT component. More...
 
def stopImpedance (self, arm)
 
def startDefaultUnstableControllers (self, ic_limbs=[], abc_limbs=[])
 Start default unstable RTCs controller mode. More...
 
def stopDefaultUnstableControllers (self, ic_limbs=[])
 Stop default unstable RTCs controller mode. More...
 
def setFootSteps (self, footstep, overwrite_fs_idx=0)
 setFootSteps More...
 
def setFootStepsWithParam (self, footstep, stepparams, overwrite_fs_idx=0)
 setFootSteps More...
 
def clearJointAngles (self)
 Cancels the commanded sequence of joint angle, by overwriting the command by the values of instant the method was invoked. More...
 
def clearJointAnglesOfGroup (self, gname)
 Cancels the commanded sequence of joint angle for the specified joint group, by overwriting the command by the values of instant the method was invoked. More...
 
def removeForceSensorOffsetRMFO (self, sensor_names=[], tm=8.0)
 remove force sensor offset by RemoveForceSensorOffset (RMFO) RTC. More...
 
def init (self, robotname="Robot", url="")
 Calls init from its superclass, which tries to connect RTCManager, looks for ModelLoader, and starts necessary RTC components. More...
 
def __init__ (self, cname="[hrpsys.py] ")
 

Public Attributes

 kinematics_only_mode
 
 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
 
bool log_use_owned_ec = False
 
 bp = None
 
 bp_svc = None
 
 bp_version = None
 
 rfu = None
 
 rfu_svc = None
 
 rfu_version = None
 
 acf = None
 
 acf_svc = None
 
 acf_version = None
 
 octd = None
 
 octd_svc = None
 
 octd_version = None
 
 ms = None
 
 hgc = None
 
 pdc = None
 
 simulation_mode = None
 
 sensors = None
 
list Groups = []
 
 hrpsys_version = None
 
bool kinematics_only_mode = False
 

Constructor & Destructor Documentation

◆ __init__()

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

Member Function Documentation

◆ activateComps()

def python.hrpsys_config.HrpsysConfigurator.activateComps (   self)

◆ calibrateInertiaSensor()

def python.hrpsys_config.HrpsysConfigurator.calibrateInertiaSensor (   self)

◆ calibrateInertiaSensorWithDialog()

def python.hrpsys_config.HrpsysConfigurator.calibrateInertiaSensorWithDialog (   self)

◆ checkEncoders()

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(), initializeJointAngle(), python.hrpsys_config.HrpsysConfigurator.isCalibDone(), python.hrpsys_config.HrpsysConfigurator.isServoOn(), python.hrpsys_config.HrpsysConfigurator.rh_svc, python.hrpsys_config.HrpsysConfigurator.sc_svc, and python.hrpsys_config.HrpsysConfigurator.servoOn().

◆ checkSimulationMode()

def python.hrpsys_config.HrpsysConfigurator.checkSimulationMode (   self)

◆ clear()

def python.hrpsys_config.HrpsysConfigurator.clear (   self)

◆ clearJointAngles()

def python.hrpsys_config.HrpsysConfigurator.clearJointAngles (   self)

Cancels the commanded sequence of joint angle, by overwriting the command by the values of instant the method was invoked.

Note that this only cancels the queue once. Icoming commands after this method is called will be processed as usual.

Returns
bool

References python.hrpsys_config.HrpsysConfigurator.seq_svc.

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

◆ clearJointAnglesOfGroup()

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

Cancels the commanded sequence of joint angle for the specified joint group, by overwriting the command by the values of instant the method was invoked.

Note that this only cancels the queue once. Icoming commands after this method is called will be processed as usual.

Parameters
gnameName of the joint group.
Returns
bool

References python.hrpsys_config.HrpsysConfigurator.clearJointAngles(), and python.hrpsys_config.HrpsysConfigurator.seq_svc.

◆ clearLog()

def python.hrpsys_config.HrpsysConfigurator.clearLog (   self)

◆ clearOfGroup()

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

Clears the Sequencer's current operation for joint groups.

References python.hrpsys_config.HrpsysConfigurator.seq_svc.

◆ connectComps()

def python.hrpsys_config.HrpsysConfigurator.connectComps (   self)

◆ connectLoggerPort()

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, python.rtm.connectPorts(), python.hrpsys_config.HrpsysConfigurator.log, and python.hrpsys_config.HrpsysConfigurator.log_svc.

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

◆ createComp()

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, python.hrpsys_config.HrpsysConfigurator.ms, and python.rtm.narrow().

◆ createComps()

def python.hrpsys_config.HrpsysConfigurator.createComps (   self)

◆ deactivateComps()

def python.hrpsys_config.HrpsysConfigurator.deactivateComps (   self)

◆ deleteComp()

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

Delete RTC component (plugins)

Parameters
compNamestr: name of component that to be deleted

References python.hrpsys_config.HrpsysConfigurator.ms.

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

◆ deleteComps()

def python.hrpsys_config.HrpsysConfigurator.deleteComps (   self)

◆ findComp()

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

◆ findComps()

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

◆ findModelLoader()

def python.hrpsys_config.HrpsysConfigurator.findModelLoader (   self)

Try to find ModelLoader.

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

◆ flat2Groups()

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.

◆ getActualState()

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 (found at https://github.com/fkanehiro/hrpsys-base/blob/3fd7671de5129101a4ade3f98e2eac39fd6b26c0/idl/RobotHardwareService.idl#L32_L57 as of version 315.11.0)
    /**
     * @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]
    };

References python.hrpsys_config.HrpsysConfigurator.rh_svc.

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

◆ getBodyInfo()

def python.hrpsys_config.HrpsysConfigurator.getBodyInfo (   self,
  url 
)

◆ getCurrentPose()

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

Returns the current physical pose of the specified joint in the joint space.

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

◆ getCurrentPosition()

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

Returns the current physical position of the specified joint in Cartesian space.

cf. getReferencePosition that returns commanded value.

eg.

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

lname: str

Parameters
lnameName 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().

◆ getCurrentRotation()

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
lnameName 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().

◆ 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
lnameName 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().

◆ getForceSensorNames()

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.

◆ getJointAngleControllerList()

def python.hrpsys_config.HrpsysConfigurator.getJointAngleControllerList (   self)

◆ getJointAngles()

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

References python.hrpsys_config.HrpsysConfigurator.sh_svc.

◆ getReferencePose()

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

Returns the current commanded pose of the specified joint in the joint space.

cf. getCurrentPose that returns physical pose.

lname: str

Parameters
lnameName 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_svc, 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().

◆ getReferencePosition()

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

Returns the current commanded position of the specified joint in Cartesian space.

cf. getCurrentPosition that returns physical value.

lname: str

Parameters
lnameName 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().

◆ getReferenceRotation()

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
lnameName 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().

◆ 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
lnameName 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().

◆ getRTCInstanceList()

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

◆ getRTCList()

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

◆ getRTCListUnstable()

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.

◆ getSensors()

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

◆ goActual()

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.

References python.hrpsys_config.HrpsysConfigurator.sh_svc.

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

◆ init()

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

◆ isCalibDone()

def python.hrpsys_config.HrpsysConfigurator.isCalibDone (   self)

◆ isServoOn()

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

◆ lengthDigitalInput()

def python.hrpsys_config.HrpsysConfigurator.lengthDigitalInput (   self)

Returns the length of digital input port.

References python.hrpsys_config.HrpsysConfigurator.rh_svc.

◆ lengthDigitalOutput()

def python.hrpsys_config.HrpsysConfigurator.lengthDigitalOutput (   self)

◆ loadPattern()

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(), and python.hrpsys_config.HrpsysConfigurator.seq_svc.

◆ parseUrl()

def python.hrpsys_config.HrpsysConfigurator.parseUrl (   self,
  url 
)

◆ playPattern()

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.

jointangles: [[float]]

Parameters
jointanglesSequence of the sets of joint angles in radian. 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: Second to complete the command. This only includes the time taken for execution (i.e. time for planning and other misc. processes are not considered).
Returns
bool:

References python.hrpsys_config.HrpsysConfigurator.seq_svc.

◆ playPatternOfGroup()

def python.hrpsys_config.HrpsysConfigurator.playPatternOfGroup (   self,
  gname,
  jointangles,
  tm 
)

Play motion pattern using a set of given trajectories that are represented by lists of joint angles.

Each trajectory aims to run within the specified time (tm), and there's no slow down between trajectories unless the next one is the last.

Example: self.playPatternOfGroup('larm', [[0.0, 0.0, -2.2689280275926285, 0.0, 0.0, 0.0], [0.0, 0.0, -1.9198621771937625, 0.0, 0.0, 0.0], [0.0, 0.0, -1.5707963, 0.0, 0.0, 0.0]], [3, 3, 10])

Parameters
gnamestr: Name of the joint group. jointangles: [[float]]
jointanglesSequence of the sets of joint angles in radian. 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 tm: [float]

Parameters
tmfloat: Sequence of the time values to complete the task, each element of which corresponds to a set of jointangles in the same order.
Returns
bool:

References python.hrpsys_config.HrpsysConfigurator.seq_svc.

◆ readDigitalInput()

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.rh_svc, and python.hrpsys_config.HrpsysConfigurator.simulation_mode.

◆ readDigitalOutput()

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.

References python.hrpsys_config.HrpsysConfigurator.rh_svc.

◆ removeForceSensorOffset()

def python.hrpsys_config.HrpsysConfigurator.removeForceSensorOffset (   self)

◆ removeForceSensorOffsetRMFO()

def python.hrpsys_config.HrpsysConfigurator.removeForceSensorOffsetRMFO (   self,
  sensor_names = [],
  tm = 8.0 
)

remove force sensor offset by RemoveForceSensorOffset (RMFO) RTC.

Parameters
sensor_names: list of sensor names to be calibrated. If not specified, all sensors are calibrated by default.
tm: calibration time[s]. 8.0[s] by default.
Returns
bool : true if set successfully, false otherwise

References python.hrpsys_config.HrpsysConfigurator.removeForceSensorOffset().

◆ saveLog()

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, and python.hrpsys_config.HrpsysConfigurator.log_svc.

◆ servoOff()

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.rh_svc, python.hrpsys_config.HrpsysConfigurator.sc_svc, and python.hrpsys_config.HrpsysConfigurator.simulation_mode.

◆ servoOn()

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(), python.hrpsys_config.HrpsysConfigurator.isServoOn(), and python.hrpsys_config.HrpsysConfigurator.rh_svc.

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

◆ setFootSteps()

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.

◆ setFootStepsWithParam()

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.

◆ setInterpolationMode()

def python.hrpsys_config.HrpsysConfigurator.setInterpolationMode (   self,
  mode 
)

Set interpolation mode.

You may need to import OpenHRP in order to pass an argument. For more info See https://github.com/fkanehiro/hrpsys-base/pull/1012#issue-160802911.

Parameters
modenew interpolation mode. Either { OpenHRP.SequencePlayerService.LINEAR, OpenHRP.SequencePlayerService.HOFFARBIB }.
Returns
true if set successfully, false otherwise

References python.hrpsys_config.HrpsysConfigurator.seq_svc.

◆ setJointAngle()

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. bool
Returns
False upon any problem during execution.

References python.hrpsys_config.HrpsysConfigurator.seq_svc.

◆ setJointAngles()

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. bool
Returns
False upon any problem during execution.

References python.hrpsys_config.HrpsysConfigurator.seq_svc.

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

◆ setJointAnglesOfGroup()

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. bool
Returns
False upon any problem during execution.

References python.hrpsys_config.HrpsysConfigurator.seq_svc, and python.hrpsys_config.HrpsysConfigurator.waitInterpolationOfGroup().

◆ setJointAnglesSequence()

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

Set all joint angles.

len(angless) should be equal to len(tms).

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
sequentiallist of angles in float: In rad
tmsequential list of time in float: Time to complete, In Second bool
Returns
False upon any problem during execution.

References python.hrpsys_config.HrpsysConfigurator.seq_svc.

◆ setJointAnglesSequenceOfGroup()

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.
sequentiallist of angles in float: In rad
tmsequential list of time in float: Time to complete, In Second bool
Returns
False upon any problem during execution.

References python.hrpsys_config.HrpsysConfigurator.seq_svc.

◆ setMaxLogLength()

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.

References python.hrpsys_config.HrpsysConfigurator.log_svc.

◆ setSelfGroups()

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, and python.hrpsys_config.HrpsysConfigurator.seq_svc.

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

◆ setSensorCalibrationJointAngles()

def python.hrpsys_config.HrpsysConfigurator.setSensorCalibrationJointAngles (   self)

◆ setTargetPose()

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. This only includes the time taken for execution (i.e. time for planning and other misc. processes are not considered).
frame_namestr: Name of the frame that this particular command references to.
Returns
bool: False if unreachable.

References python.hrpsys_config.HrpsysConfigurator.seq_svc, and python.hrpsys_config.HrpsysConfigurator.setTargetPoseRelative().

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

◆ 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.

Note that because of "relative" nature, the method waits for the commands run previously to finish. Do not get confused with the "wait" argument, which regulates all subsequent commands, not previous ones.

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 that is to be manipulated.
eenamestr: Name of the joint that the manipulated joint group references to.
dxfloat: In meter.
dyfloat: In meter.
dzfloat: In meter.
drfloat: In radian.
dpfloat: In radian.
dwfloat: In radian.
tmfloat: Second to complete the command. This only includes the time taken for execution (i.e. time for planning and other misc. processes are not considered).
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(), list(), python.hrpsys_config.HrpsysConfigurator.setTargetPose(), and python.hrpsys_config.HrpsysConfigurator.waitInterpolationOfGroup().

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

◆ setupLogger()

def python.hrpsys_config.HrpsysConfigurator.setupLogger (   self,
  maxLength = 4000 
)

◆ startAutoBalancer()

def python.hrpsys_config.HrpsysConfigurator.startAutoBalancer (   self,
  limbs = None 
)

Start AutoBalancer mode.

Parameters
limbslist of end-effector name to control. If Groups has rarm and larm, rleg, lleg, rarm, larm by default. If Groups is not defined or Groups does not have rarm and larm, rleg and lleg by default.

References python.hrpsys_config.HrpsysConfigurator.Groups.

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

◆ startDefaultUnstableControllers()

def python.hrpsys_config.HrpsysConfigurator.startDefaultUnstableControllers (   self,
  ic_limbs = [],
  abc_limbs = [] 
)

Start default unstable RTCs controller mode.

Currently Stabilzier, AutoBalancer, and ImpedanceController are started based on "Groups" setting. If ic_limbs or abc_limbs is not specified, default setting is set according to "Groups". By default, If the robot has an arm, start impedance for the arm. If the robot has a leg, start st and autobalancer. autobalancer's fixed limbs are all existing arms and legs. Use cases: Biped robot (leg only) : no impedance, start st, start autobalancer with ["rleg", "lleg"]. Dual-arm robot (arm only) : start impedance ["rarm", "larm"], no st, no autobalancer. Dual-arm+biped robot (=humanoid robot) : start impedance ["rarm", "larm"], start st, start autobalancer with ["rleg", "lleg", "rarm", "larm"]. Quadruped robot : same as "humanoid robot" by default.

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

◆ startImpedance()

def python.hrpsys_config.HrpsysConfigurator.startImpedance (   self,
  arm,
  kwargs 
)

Enable the ImpedanceController RT component.

This method internally calls startImpedance-*, hrpsys version-specific method.

: hrpsys version greather than 315.2.0. : ImpedanceController RTC to be activated on the robot's controller. : From 315.2.0 onward, following arguments are dropped and can be set by self.seq_svc.setWrenches instead of this method. See an example at https://github.com/fkanehiro/hrpsys-base/pull/434/files#diff-6204f002204dd9ae80f203901f155fa9R44:

  • ref_force[fx, fy, fz] (unit: N) and ref_moment[tx, ty, tz] (unit: Nm) can be set via self.seq_svc.setWrenches. For example:

    self.seq_svc.setWrenches([0, 0, 0, 0, 0, 0, fx, fy, fz, tx, ty, tz, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,])

    setWrenches takes 6 values per sensor, so the robot in the example above has 4 sensors where each line represents a sensor. See this link (https://github.com/fkanehiro/hrpsys-base/pull/434/files) for a concrete example.

Parameters
armName of the kinematic group (i.e. self.Groups[n][0]).
kwargsThis varies depending on the version of hrpsys your robot's controller runs on (which you can find by "self.hrpsys_version" command). For instance, if your hrpsys is 315.10.1, refer to "startImpedance_315_4" method.

References python.hrpsys_config.HrpsysConfigurator.configurator_name, python.hrpsys_config.HrpsysConfigurator.hrpsys_version, and python.hrpsys_config.HrpsysConfigurator.startImpedance_315_4().

◆ startImpedance_315_4()

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()

Parameters
force_gain,moment_gainmultipliers to the eef offset position vel_p and orientation vel_r. 3-dimensional vector (then converted internally into a diagonal matrix).

References python.hrpsys_config.HrpsysConfigurator.configurator_name.

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

◆ startStabilizer()

def python.hrpsys_config.HrpsysConfigurator.startStabilizer (   self)

◆ stopAutoBalancer()

def python.hrpsys_config.HrpsysConfigurator.stopAutoBalancer (   self)

◆ stopDefaultUnstableControllers()

def python.hrpsys_config.HrpsysConfigurator.stopDefaultUnstableControllers (   self,
  ic_limbs = [] 
)

Stop default unstable RTCs controller mode.

Currently Stabilzier, AutoBalancer, and ImpedanceController are stopped based on "Groups" setting. Please see documentation of startDefaultUnstableControllers().

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

◆ stopImpedance()

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

◆ stopImpedance_315_4()

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

stop impedance mode

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

◆ stopStabilizer()

def python.hrpsys_config.HrpsysConfigurator.stopStabilizer (   self)

◆ waitForModelLoader()

def python.hrpsys_config.HrpsysConfigurator.waitForModelLoader (   self)

◆ waitForRobotHardware()

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.waitForRTCManagerAndRobotHardware().

◆ waitForRTCManager()

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.waitForRTCManagerAndRobotHardware().

◆ waitForRTCManagerAndRoboHardware()

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

◆ waitForRTCManagerAndRobotHardware()

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

◆ waitInterpolation()

def python.hrpsys_config.HrpsysConfigurator.waitInterpolation (   self)

Lets SequencePlayer wait until the movement currently happening to finish.

References python.hrpsys_config.HrpsysConfigurator.seq_svc.

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

◆ waitInterpolationOfGroup()

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.

References python.hrpsys_config.HrpsysConfigurator.seq_svc.

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

◆ writeDigitalOutput()

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(), python.hrpsys_config.HrpsysConfigurator.rh_svc, and python.hrpsys_config.HrpsysConfigurator.simulation_mode.

◆ writeDigitalOutputWithMask()

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(), python.hrpsys_config.HrpsysConfigurator.rh_svc, and python.hrpsys_config.HrpsysConfigurator.simulation_mode.

Member Data Documentation

◆ abc

python.hrpsys_config.HrpsysConfigurator.abc = None
static

◆ abc_version

python.hrpsys_config.HrpsysConfigurator.abc_version = None
static

◆ acf

python.hrpsys_config.HrpsysConfigurator.acf = None
static

◆ acf_svc

python.hrpsys_config.HrpsysConfigurator.acf_svc = None
static

◆ acf_version

python.hrpsys_config.HrpsysConfigurator.acf_version = None
static

◆ bp

python.hrpsys_config.HrpsysConfigurator.bp = None
static

◆ bp_svc

python.hrpsys_config.HrpsysConfigurator.bp_svc = None
static

◆ bp_version

python.hrpsys_config.HrpsysConfigurator.bp_version = None
static

◆ co

python.hrpsys_config.HrpsysConfigurator.co = None
static

◆ co_svc

python.hrpsys_config.HrpsysConfigurator.co_svc = None
static

◆ co_version

python.hrpsys_config.HrpsysConfigurator.co_version = None
static

◆ configurator_name

python.hrpsys_config.HrpsysConfigurator.configurator_name

Referenced by python.hrpsys_config.HrpsysConfigurator.checkEncoders(), python.hrpsys_config.HrpsysConfigurator.checkSimulationMode(), python.hrpsys_config.HrpsysConfigurator.connectComps(), python.hrpsys_config.HrpsysConfigurator.connectLoggerPort(), python.hrpsys_config.HrpsysConfigurator.createComp(), python.hrpsys_config.HrpsysConfigurator.createComps(), python.hrpsys_config.HrpsysConfigurator.deleteComps(), python.hrpsys_config.HrpsysConfigurator.findComp(), python.hrpsys_config.HrpsysConfigurator.findComps(), python.hrpsys_config.HrpsysConfigurator.getBodyInfo(), python.hrpsys_config.HrpsysConfigurator.getRTCInstanceList(), python.hrpsys_config.HrpsysConfigurator.init(), python.hrpsys_config.HrpsysConfigurator.isServoOn(), python.hrpsys_config.HrpsysConfigurator.saveLog(), python.hrpsys_config.HrpsysConfigurator.servoOff(), python.hrpsys_config.HrpsysConfigurator.servoOn(), python.hrpsys_config.HrpsysConfigurator.setupLogger(), python.hrpsys_config.HrpsysConfigurator.startDefaultUnstableControllers(), python.hrpsys_config.HrpsysConfigurator.startImpedance(), python.hrpsys_config.HrpsysConfigurator.startImpedance_315_4(), python.hrpsys_config.HrpsysConfigurator.stopDefaultUnstableControllers(), python.hrpsys_config.HrpsysConfigurator.stopImpedance(), python.hrpsys_config.HrpsysConfigurator.waitForModelLoader(), python.hrpsys_config.HrpsysConfigurator.waitForRobotHardware(), python.hrpsys_config.HrpsysConfigurator.waitForRTCManager(), and python.hrpsys_config.HrpsysConfigurator.waitForRTCManagerAndRoboHardware().

◆ el

python.hrpsys_config.HrpsysConfigurator.el = None
static

◆ el_svc

python.hrpsys_config.HrpsysConfigurator.el_svc = None
static

◆ el_version

python.hrpsys_config.HrpsysConfigurator.el_version = None
static

◆ ep_svc

python.hrpsys_config.HrpsysConfigurator.ep_svc = None
static

◆ es

python.hrpsys_config.HrpsysConfigurator.es = None
static

◆ es_svc

python.hrpsys_config.HrpsysConfigurator.es_svc = None
static

◆ es_version

python.hrpsys_config.HrpsysConfigurator.es_version = None
static

◆ fk

python.hrpsys_config.HrpsysConfigurator.fk = None
static

◆ fk_svc

python.hrpsys_config.HrpsysConfigurator.fk_svc = None
static

◆ fk_version

python.hrpsys_config.HrpsysConfigurator.fk_version = None
static

◆ gc

python.hrpsys_config.HrpsysConfigurator.gc = None
static

◆ gc_svc

python.hrpsys_config.HrpsysConfigurator.gc_svc = None
static

◆ gc_version

python.hrpsys_config.HrpsysConfigurator.gc_version = None
static

◆ Groups

list python.hrpsys_config.HrpsysConfigurator.Groups = []
static

◆ hes

python.hrpsys_config.HrpsysConfigurator.hes = None
static

◆ hes_svc

python.hrpsys_config.HrpsysConfigurator.hes_svc = None
static

◆ hes_version

python.hrpsys_config.HrpsysConfigurator.hes_version = None
static

◆ hgc

python.hrpsys_config.HrpsysConfigurator.hgc = None
static

◆ hrpsys_version

python.hrpsys_config.HrpsysConfigurator.hrpsys_version = None
static

◆ ic

python.hrpsys_config.HrpsysConfigurator.ic = None
static

◆ ic_version

python.hrpsys_config.HrpsysConfigurator.ic_version = None
static

◆ kf

python.hrpsys_config.HrpsysConfigurator.kf = None
static

◆ kf_version

python.hrpsys_config.HrpsysConfigurator.kf_version = None
static

◆ kinematics_only_mode [1/2]

bool python.hrpsys_config.HrpsysConfigurator.kinematics_only_mode = False
static

◆ kinematics_only_mode [2/2]

python.hrpsys_config.HrpsysConfigurator.kinematics_only_mode

◆ log

python.hrpsys_config.HrpsysConfigurator.log = None
static

◆ log_svc

python.hrpsys_config.HrpsysConfigurator.log_svc = None
static

◆ log_use_owned_ec

bool python.hrpsys_config.HrpsysConfigurator.log_use_owned_ec = False
static

◆ log_version

python.hrpsys_config.HrpsysConfigurator.log_version = None
static

◆ ms

python.hrpsys_config.HrpsysConfigurator.ms = None
static

◆ octd

python.hrpsys_config.HrpsysConfigurator.octd = None
static

◆ octd_svc

python.hrpsys_config.HrpsysConfigurator.octd_svc = None
static

◆ octd_version

python.hrpsys_config.HrpsysConfigurator.octd_version = None
static

◆ pdc

python.hrpsys_config.HrpsysConfigurator.pdc = None
static

◆ rfu

python.hrpsys_config.HrpsysConfigurator.rfu = None
static

◆ rfu_svc

python.hrpsys_config.HrpsysConfigurator.rfu_svc = None
static

◆ rfu_version

python.hrpsys_config.HrpsysConfigurator.rfu_version = None
static

◆ rh

python.hrpsys_config.HrpsysConfigurator.rh = None
static

◆ rh_svc

python.hrpsys_config.HrpsysConfigurator.rh_svc = None
static

◆ rh_version

python.hrpsys_config.HrpsysConfigurator.rh_version = None
static

◆ rmfo

python.hrpsys_config.HrpsysConfigurator.rmfo = None
static

◆ rmfo_version

python.hrpsys_config.HrpsysConfigurator.rmfo_version = None
static

◆ sc

python.hrpsys_config.HrpsysConfigurator.sc = None
static

◆ sc_svc

python.hrpsys_config.HrpsysConfigurator.sc_svc = None
static

◆ sc_version

python.hrpsys_config.HrpsysConfigurator.sc_version = None
static

◆ sensors

python.hrpsys_config.HrpsysConfigurator.sensors = None
static

◆ seq

python.hrpsys_config.HrpsysConfigurator.seq = None
static

◆ seq_svc

python.hrpsys_config.HrpsysConfigurator.seq_svc = None
static

◆ seq_version

python.hrpsys_config.HrpsysConfigurator.seq_version = None
static

◆ sh

python.hrpsys_config.HrpsysConfigurator.sh = None
static

◆ sh_svc

python.hrpsys_config.HrpsysConfigurator.sh_svc = None
static

◆ sh_version

python.hrpsys_config.HrpsysConfigurator.sh_version = None
static

◆ simulation_mode

python.hrpsys_config.HrpsysConfigurator.simulation_mode = None
static

◆ st

python.hrpsys_config.HrpsysConfigurator.st = None
static

◆ st_version

python.hrpsys_config.HrpsysConfigurator.st_version = None
static

◆ tc

python.hrpsys_config.HrpsysConfigurator.tc = None
static

◆ tc_svc

python.hrpsys_config.HrpsysConfigurator.tc_svc = None
static

◆ tc_version

python.hrpsys_config.HrpsysConfigurator.tc_version = None
static

◆ te

python.hrpsys_config.HrpsysConfigurator.te = None
static

◆ te_svc

python.hrpsys_config.HrpsysConfigurator.te_svc = None
static

◆ te_version

python.hrpsys_config.HrpsysConfigurator.te_version = None
static

◆ tf

python.hrpsys_config.HrpsysConfigurator.tf = None
static

◆ tf_version

python.hrpsys_config.HrpsysConfigurator.tf_version = None
static

◆ tl

python.hrpsys_config.HrpsysConfigurator.tl = None
static

◆ tl_svc

python.hrpsys_config.HrpsysConfigurator.tl_svc = None
static

◆ tl_version

python.hrpsys_config.HrpsysConfigurator.tl_version = None
static

◆ vs

python.hrpsys_config.HrpsysConfigurator.vs = None
static

◆ vs_version

python.hrpsys_config.HrpsysConfigurator.vs_version = None
static

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