Basic RT components and utilities
315.15.0
|
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 |
def python.hrpsys_config.HrpsysConfigurator.__init__ | ( | self, | |
cname = "[hrpsys.py] " |
|||
) |
References python.rtm.initCORBA().
def python.hrpsys_config.HrpsysConfigurator.activateComps | ( | self | ) |
Activate components(plugins)
References python.hrpsys_config.HrpsysConfigurator.getRTCInstanceList().
Referenced by python.hrpsys_config.HrpsysConfigurator.init().
def python.hrpsys_config.HrpsysConfigurator.calibrateInertiaSensor | ( | self | ) |
Calibrate inertia sensor.
References python.hrpsys_config.HrpsysConfigurator.rh_svc.
Referenced by python.hrpsys_config.HrpsysConfigurator.calibrateInertiaSensorWithDialog().
def python.hrpsys_config.HrpsysConfigurator.calibrateInertiaSensorWithDialog | ( | self | ) |
Calibrate inertia sensor with dialog and setting calibration pose.
References python.hrpsys_config.HrpsysConfigurator.calibrateInertiaSensor(), and python.hrpsys_config.HrpsysConfigurator.setSensorCalibrationJointAngles().
def python.hrpsys_config.HrpsysConfigurator.checkEncoders | ( | self, | |
jname = 'all' , |
|||
option = '' |
|||
) |
Run the encoder checking sequence for specified joints, run goActual and turn on servos.
jname | str: The value 'all' works iteratively for all servos. |
option | str: 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().
def python.hrpsys_config.HrpsysConfigurator.checkSimulationMode | ( | self | ) |
Check if this is running as simulation.
References python.hrpsys_config.HrpsysConfigurator.configurator_name, python.hrpsys_config.HrpsysConfigurator.ep_svc, python.rtm.findRTC(), python.hrpsys_config.HrpsysConfigurator.hgc, python.rtm.narrow(), python.hrpsys_config.HrpsysConfigurator.pdc, python.hrpsys_config.HrpsysConfigurator.rh, python.hrpsys_config.HrpsysConfigurator.rh_svc, and python.hrpsys_config.HrpsysConfigurator.simulation_mode.
Referenced by python.hrpsys_config.HrpsysConfigurator.waitForRTCManagerAndRobotHardware().
def python.hrpsys_config.HrpsysConfigurator.clear | ( | self | ) |
Clears the Sequencer's current operation.
Works for joint groups too.
Discussed in https://github.com/fkanehiro/hrpsys-base/issues/158 Examples is found in a unit test: https://github.com/start-jsk/rtmros_hironx/blob/bb0672be3e03e5366e03fe50520e215302b8419f/hironx_ros_bridge/test/test_hironx.py#L293
References python.hrpsys_config.HrpsysConfigurator.seq_svc.
Referenced by python.hrpsys_config.HrpsysConfigurator.clearLog(), and python.hrpsys_config.HrpsysConfigurator.setupLogger().
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.
References python.hrpsys_config.HrpsysConfigurator.seq_svc.
Referenced by python.hrpsys_config.HrpsysConfigurator.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.
gname | Name of the joint group. |
References python.hrpsys_config.HrpsysConfigurator.clearJointAngles(), and python.hrpsys_config.HrpsysConfigurator.seq_svc.
def python.hrpsys_config.HrpsysConfigurator.clearLog | ( | self | ) |
Clear logger's buffer.
References python.hrpsys_config.HrpsysConfigurator.clear(), and python.hrpsys_config.HrpsysConfigurator.log_svc.
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.
def python.hrpsys_config.HrpsysConfigurator.connectComps | ( | self | ) |
Connect components(plugins)
References python.hrpsys_config.HrpsysConfigurator.configurator_name, python.rtm.connectPorts(), python.hrpsys_config.HrpsysConfigurator.fk, python.hrpsys_config.HrpsysConfigurator.getJointAngleControllerList(), python.hrpsys_config.HrpsysConfigurator.hgc, python.hrpsys_config.HrpsysConfigurator.pdc, python.hrpsys_config.HrpsysConfigurator.rh, python.hrpsys_config.HrpsysConfigurator.seq, python.hrpsys_config.HrpsysConfigurator.sh, and python.hrpsys_config.HrpsysConfigurator.simulation_mode.
Referenced by python.hrpsys_config.HrpsysConfigurator.init().
def python.hrpsys_config.HrpsysConfigurator.connectLoggerPort | ( | self, | |
artc, | |||
sen_name, | |||
log_name = None |
|||
) |
Connect port to logger.
artc | object: object of component that contains sen_name port |
sen_name | str: name of port for logging |
log_name | str: 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().
def python.hrpsys_config.HrpsysConfigurator.createComp | ( | self, | |
compName, | |||
instanceName | |||
) |
Create RTC component (plugins)
instanceName | str: name of instance, choose one of https://github.com/fkanehiro/hrpsys-base/tree/master/rtc |
comName | str: name of component that to be created. |
References python.hrpsys_config.HrpsysConfigurator.configurator_name, python.hrpsys_config.HrpsysConfigurator.ms, and python.rtm.narrow().
def python.hrpsys_config.HrpsysConfigurator.createComps | ( | self | ) |
Create components(plugins) in getRTCList()
References python.hrpsys_config.HrpsysConfigurator.configurator_name, and python.hrpsys_config.HrpsysConfigurator.getRTCList().
Referenced by python.hrpsys_config.HrpsysConfigurator.init().
def python.hrpsys_config.HrpsysConfigurator.deactivateComps | ( | self | ) |
Deactivate components(plugins)
References python.hrpsys_config.HrpsysConfigurator.getRTCInstanceList().
Referenced by python.hrpsys_config.HrpsysConfigurator.deleteComps().
def python.hrpsys_config.HrpsysConfigurator.deleteComp | ( | self, | |
compName | |||
) |
Delete RTC component (plugins)
compName | str: name of component that to be deleted |
References python.hrpsys_config.HrpsysConfigurator.ms.
Referenced by python.hrpsys_config.HrpsysConfigurator.deleteComps().
def python.hrpsys_config.HrpsysConfigurator.deleteComps | ( | self | ) |
Delete components(plugins) in getRTCInstanceList()
References python.hrpsys_config.HrpsysConfigurator.configurator_name, python.hrpsys_config.HrpsysConfigurator.deactivateComps(), python.hrpsys_config.HrpsysConfigurator.deleteComp(), python.hrpsys_config.HrpsysConfigurator.getRTCInstanceList(), and python.hrpsys_config.HrpsysConfigurator.rh.
def python.hrpsys_config.HrpsysConfigurator.findComp | ( | self, | |
compName, | |||
instanceName, | |||
max_timeout_count = 10 , |
|||
verbose = True |
|||
) |
Find component(plugin)
compName | str: component name |
instanceName | str: 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 |
|||
) |
Check if all components in getRTCList() are created.
References python.hrpsys_config.HrpsysConfigurator.configurator_name, and python.hrpsys_config.HrpsysConfigurator.getRTCList().
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.
flatList | list: single dimension list with its length of 15 |
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.
/** * @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().
def python.hrpsys_config.HrpsysConfigurator.getBodyInfo | ( | self, | |
url | |||
) |
Get bodyInfo.
References python.hrpsys_config.HrpsysConfigurator.configurator_name, and python.hrpsys_config.HrpsysConfigurator.parseUrl().
Referenced by python.hrpsys_config.HrpsysConfigurator.getSensors().
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
lname | Name of the link. |
frame_name | str: set reference frame name (from 315.2.5) : list of float |
[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().
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
lname | Name of the link. |
frame_name | str: set reference frame name (from 315.2.5) : list of float |
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
lname | Name of the link. |
frame_name | str: set reference frame name (from 315.2.5) : list of float |
[[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
lname | Name of the link. |
frame_name | str: set reference frame name (from 315.2.5) : list of float |
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.
def python.hrpsys_config.HrpsysConfigurator.getJointAngleControllerList | ( | self | ) |
Get list of controller list that need to control joint angles.
References python.hrpsys_config.HrpsysConfigurator.abc, python.hrpsys_config.HrpsysConfigurator.co, python.hrpsys_config.HrpsysConfigurator.el, python.hrpsys_config.HrpsysConfigurator.es, python.hrpsys_config.HrpsysConfigurator.gc, python.hrpsys_config.HrpsysConfigurator.hes, python.hrpsys_config.HrpsysConfigurator.ic, python.hrpsys_config.HrpsysConfigurator.st, and python.hrpsys_config.HrpsysConfigurator.tc.
Referenced by python.hrpsys_config.HrpsysConfigurator.connectComps().
def python.hrpsys_config.HrpsysConfigurator.getJointAngles | ( | self | ) |
Returns the commanded joint angle values.
Note that it's not the physical state of the robot's joints, which can be obtained by getActualState().angle.
References python.hrpsys_config.HrpsysConfigurator.sh_svc.
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
lname | Name of the link. |
frame_name | str: set reference frame name (from 315.2.5) : list of float |
[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().
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
lname | Name of the link. |
frame_name | str: set reference frame name (from 315.2.5) : list of float |
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
lname | Name of the link. |
frame_name | str: set reference frame name (from 315.2.5) : list of float |
[[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
lname | Name of the link. |
frame_name | str: set reference frame name (from 315.2.5) : list of float |
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 |
|||
) |
Get list of RTC Instance.
References python.hrpsys_config.HrpsysConfigurator.configurator_name, python.hrpsys_config.HrpsysConfigurator.getRTCList(), and python.hrpsys_config.HrpsysConfigurator.rh.
Referenced by python.hrpsys_config.HrpsysConfigurator.activateComps(), python.hrpsys_config.HrpsysConfigurator.deactivateComps(), and python.hrpsys_config.HrpsysConfigurator.deleteComps().
def python.hrpsys_config.HrpsysConfigurator.getRTCList | ( | self | ) |
Get list of available STABLE components.
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.
def python.hrpsys_config.HrpsysConfigurator.getSensors | ( | self, | |
url | |||
) |
Get list of sensors.
url | str: 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.
References python.hrpsys_config.HrpsysConfigurator.sh_svc.
Referenced by python.hrpsys_config.HrpsysConfigurator.checkEncoders(), and python.hrpsys_config.HrpsysConfigurator.servoOn().
def python.hrpsys_config.HrpsysConfigurator.init | ( | self, | |
robotname = "Robot" , |
|||
url = "" |
|||
) |
Calls init from its superclass, which tries to connect RTCManager, looks for ModelLoader, and starts necessary RTC components.
Also runs config, logger. Also internally calls setSelfGroups().
robotname: str url: str
References python.hrpsys_config.HrpsysConfigurator.activateComps(), python.hrpsys_config.HrpsysConfigurator.configurator_name, python.hrpsys_config.HrpsysConfigurator.connectComps(), python.hrpsys_config.HrpsysConfigurator.createComps(), python.hrpsys_config.HrpsysConfigurator.fk, python.hrpsys_config.HrpsysConfigurator.getSensors(), python.hrpsys_config.HrpsysConfigurator.hrpsys_version, python.hrpsys_config.HrpsysConfigurator.sensors, python.hrpsys_config.HrpsysConfigurator.setSelfGroups(), python.hrpsys_config.HrpsysConfigurator.setupLogger(), python.hrpsys_config.HrpsysConfigurator.waitForModelLoader(), and python.hrpsys_config.HrpsysConfigurator.waitForRTCManagerAndRobotHardware().
def python.hrpsys_config.HrpsysConfigurator.isCalibDone | ( | self | ) |
Check whether joints have been calibrated.
References python.hrpsys_config.HrpsysConfigurator.rh_svc, and 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.
jname | str: 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. |
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.
References python.hrpsys_config.HrpsysConfigurator.rh_svc.
def python.hrpsys_config.HrpsysConfigurator.lengthDigitalOutput | ( | self | ) |
Returns the length of digital output port.
References python.hrpsys_config.HrpsysConfigurator.rh_svc.
Referenced by python.hrpsys_config.HrpsysConfigurator.writeDigitalOutput(), and python.hrpsys_config.HrpsysConfigurator.writeDigitalOutputWithMask().
def python.hrpsys_config.HrpsysConfigurator.loadPattern | ( | self, | |
fname, | |||
tm | |||
) |
Load a pattern file that is created offline.
Format of the pattern file:
t0 j0 j1 j2...jn t1 j0 j1 j2...jn : tn j0 j1 j2...jn
fname | str: Name of the pattern file. |
tm | float: - The time to take for the 1st line. |
References python.hrpsys_config.HrpsysConfigurator.parseUrl(), and python.hrpsys_config.HrpsysConfigurator.seq_svc.
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.
jointangles: [[float]]
jointangles | Sequence 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. |
rpy | list of float: Orientation in rpy. |
zmp | list of float: TODO: description |
tm | float: 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). |
References python.hrpsys_config.HrpsysConfigurator.seq_svc.
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])
gname | str: Name of the joint group. jointangles: [[float]] |
jointangles | Sequence 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]
tm | float: Sequence of the time values to complete the task, each element of which corresponds to a set of jointangles in the same order. |
References python.hrpsys_config.HrpsysConfigurator.seq_svc.
def python.hrpsys_config.HrpsysConfigurator.readDigitalInput | ( | self | ) |
Read data from Digital Input.
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.
References python.hrpsys_config.HrpsysConfigurator.rh_svc, and 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.
References python.hrpsys_config.HrpsysConfigurator.rh_svc.
def python.hrpsys_config.HrpsysConfigurator.removeForceSensorOffset | ( | self | ) |
remove force sensor offset
References python.hrpsys_config.HrpsysConfigurator.rh_svc.
Referenced by python.hrpsys_config.HrpsysConfigurator.removeForceSensorOffsetRMFO().
def python.hrpsys_config.HrpsysConfigurator.removeForceSensorOffsetRMFO | ( | self, | |
sensor_names = [] , |
|||
tm = 8.0 |
|||
) |
remove force sensor offset by RemoveForceSensorOffset (RMFO) RTC.
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. |
References python.hrpsys_config.HrpsysConfigurator.removeForceSensorOffset().
def python.hrpsys_config.HrpsysConfigurator.saveLog | ( | self, | |
fname = 'sample' |
|||
) |
Save log to the given file name.
fname | str: name of the file |
References python.hrpsys_config.HrpsysConfigurator.configurator_name, and python.hrpsys_config.HrpsysConfigurator.log_svc.
def python.hrpsys_config.HrpsysConfigurator.servoOff | ( | self, | |
jname = 'all' , |
|||
wait = True |
|||
) |
Turn off servos.
jname | str: The value 'all' works iteratively for all servos. |
wait | bool: Wait for user's confirmation via GUI |
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.
def python.hrpsys_config.HrpsysConfigurator.servoOn | ( | self, | |
jname = 'all' , |
|||
destroy = 1 , |
|||
tm = 3 |
|||
) |
Turn on servos.
Joints need to be calibrated (otherwise error returns).
jname | str: The value 'all' works iteratively for all servos. |
destroy | int: Not used. |
tm | float: Second to complete. |
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().
def python.hrpsys_config.HrpsysConfigurator.setFootSteps | ( | self, | |
footstep, | |||
overwrite_fs_idx = 0 |
|||
) |
setFootSteps
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
footstep | : FootstepSequence. |
stepparams | : StepParamSeuqnce. |
overwrite_fs_idx | : Index to be overwritten. overwrite_fs_idx is used only in walking. |
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.
mode | new interpolation mode. Either { OpenHRP.SequencePlayerService.LINEAR, OpenHRP.SequencePlayerService.HOFFARBIB }. |
References python.hrpsys_config.HrpsysConfigurator.seq_svc.
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.
jname | str: name of joint |
angle | float: In degree. |
tm | float: Time to complete. bool |
References python.hrpsys_config.HrpsysConfigurator.seq_svc.
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.
angles | list of float: In degree. |
tm | float: Time to complete. bool |
References python.hrpsys_config.HrpsysConfigurator.seq_svc.
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.
gname | str: Name of the joint group. |
pose | list of float: list of positions and orientations |
tm | float: Time to complete. |
wait | bool: If true, all other subsequent commands wait until the movement commanded by this method call finishes. bool |
References python.hrpsys_config.HrpsysConfigurator.seq_svc, and python.hrpsys_config.HrpsysConfigurator.waitInterpolationOfGroup().
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.
sequential | list of angles in float: In rad |
tm | sequential list of time in float: Time to complete, In Second bool |
References python.hrpsys_config.HrpsysConfigurator.seq_svc.
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.
gname | str: Name of the joint group. |
sequential | list of angles in float: In rad |
tm | sequential list of time in float: Time to complete, In Second bool |
References python.hrpsys_config.HrpsysConfigurator.seq_svc.
def python.hrpsys_config.HrpsysConfigurator.setMaxLogLength | ( | self, | |
length | |||
) |
Set logger's buffer.
length | int: 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.
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().
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.rh_svc, 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.
gname | str: Name of the joint group. |
pos | list of float: In meter. |
rpy | list of float: In radian. |
tm | float: 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_name | str: Name of the frame that this particular command references to. |
References python.hrpsys_config.HrpsysConfigurator.seq_svc, and 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.
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)
gname | str: Name of the joint group that is to be manipulated. |
eename | str: Name of the joint that the manipulated joint group references to. |
dx | float: In meter. |
dy | float: In meter. |
dz | float: In meter. |
dr | float: In radian. |
dp | float: In radian. |
dw | float: In radian. |
tm | float: 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). |
wait | bool: If true, all other subsequent commands wait until the movement commanded by this method call finishes. |
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().
def python.hrpsys_config.HrpsysConfigurator.setupLogger | ( | self, | |
maxLength = 4000 |
|||
) |
Setup logging function.
maxLength | : max length of data from DataLogger.h #define DEFAULT_MAX_LOG_LENGTH (200*20) if the robot running at 200hz (5msec) 4000 means 20 secs |
References python.hrpsys_config.HrpsysConfigurator.abc, python.hrpsys_config.HrpsysConfigurator.clear(), python.hrpsys_config.HrpsysConfigurator.configurator_name, python.hrpsys_config.HrpsysConfigurator.connectLoggerPort(), python.hrpsys_config.HrpsysConfigurator.el, python.hrpsys_config.HrpsysConfigurator.ic, python.hrpsys_config.HrpsysConfigurator.kf, python.hrpsys_config.HrpsysConfigurator.log, python.hrpsys_config.HrpsysConfigurator.log_svc, python.hrpsys_config.HrpsysConfigurator.log_use_owned_ec, python.hrpsys_config.HrpsysConfigurator.rfu, python.hrpsys_config.HrpsysConfigurator.rh, python.hrpsys_config.HrpsysConfigurator.rmfo, python.hrpsys_config.HrpsysConfigurator.sensors, python.hrpsys_config.HrpsysConfigurator.sh, python.hrpsys_config.HrpsysConfigurator.simulation_mode, and python.hrpsys_config.HrpsysConfigurator.st.
Referenced by python.hrpsys_config.HrpsysConfigurator.init().
def python.hrpsys_config.HrpsysConfigurator.startAutoBalancer | ( | self, | |
limbs = None |
|||
) |
Start AutoBalancer mode.
limbs | list 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().
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().
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.
arm | Name of the kinematic group (i.e. self.Groups[n][0]). |
kwargs | This 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().
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()
force_gain,moment_gain | multipliers 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().
def python.hrpsys_config.HrpsysConfigurator.startStabilizer | ( | self | ) |
Start Stabilzier mode.
Referenced by python.hrpsys_config.HrpsysConfigurator.startDefaultUnstableControllers().
def python.hrpsys_config.HrpsysConfigurator.stopAutoBalancer | ( | self | ) |
Stop AutoBalancer mode.
Referenced by python.hrpsys_config.HrpsysConfigurator.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().
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 | ) |
Stop Stabilzier mode.
References python.hrpsys_config.HrpsysConfigurator.startImpedance_315_4().
Referenced by python.hrpsys_config.HrpsysConfigurator.stopDefaultUnstableControllers().
def python.hrpsys_config.HrpsysConfigurator.waitForModelLoader | ( | self | ) |
Wait for ModelLoader.
References python.hrpsys_config.HrpsysConfigurator.configurator_name, and python.hrpsys_config.HrpsysConfigurator.findModelLoader().
Referenced by python.hrpsys_config.HrpsysConfigurator.init().
def python.hrpsys_config.HrpsysConfigurator.waitForRobotHardware | ( | self, | |
robotname = "Robot" |
|||
) |
Wait for RobotHardware is exists and activated.
robotname | str: 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().
def python.hrpsys_config.HrpsysConfigurator.waitForRTCManager | ( | self, | |
managerhost = nshost |
|||
) |
Wait for RTC manager.
managerhost | str: 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().
def python.hrpsys_config.HrpsysConfigurator.waitForRTCManagerAndRoboHardware | ( | self, | |
robotname = "Robot" , |
|||
managerhost = nshost |
|||
) |
def python.hrpsys_config.HrpsysConfigurator.waitForRTCManagerAndRobotHardware | ( | self, | |
robotname = "Robot" , |
|||
managerhost = nshost |
|||
) |
Wait for both RTC Manager (waitForRTCManager()) and RobotHardware (waitForRobotHardware())
managerhost | str: name of host computer that manager is running |
robotname | str: 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(), and python.hrpsys_config.HrpsysConfigurator.waitForRTCManagerAndRoboHardware().
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().
def python.hrpsys_config.HrpsysConfigurator.waitInterpolationOfGroup | ( | self, | |
gname | |||
) |
Lets SequencePlayer wait until the movement currently happening to finish.
gname | str: 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().
def python.hrpsys_config.HrpsysConfigurator.writeDigitalOutput | ( | self, | |
dout | |||
) |
Using writeDigitalOutputWithMask is recommended for the less data transport.
dout | list 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.
References python.hrpsys_config.HrpsysConfigurator.lengthDigitalOutput(), python.hrpsys_config.HrpsysConfigurator.rh_svc, 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]
dout | list of int: List of bits, length of 32 bits where elements are 0 or 1. |
mask | list of int: List of masking bits, length of 32 bits where elements are 0 or 1. |
References python.hrpsys_config.HrpsysConfigurator.lengthDigitalOutput(), python.hrpsys_config.HrpsysConfigurator.rh_svc, and python.hrpsys_config.HrpsysConfigurator.simulation_mode.
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
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().
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
Referenced by python.hrpsys_config.HrpsysConfigurator.flat2Groups(), python.hrpsys_config.HrpsysConfigurator.getCurrentPose(), python.hrpsys_config.HrpsysConfigurator.getCurrentPosition(), python.hrpsys_config.HrpsysConfigurator.getCurrentRotation(), python.hrpsys_config.HrpsysConfigurator.getCurrentRPY(), python.hrpsys_config.HrpsysConfigurator.getReferencePose(), python.hrpsys_config.HrpsysConfigurator.getReferencePosition(), python.hrpsys_config.HrpsysConfigurator.getReferenceRotation(), python.hrpsys_config.HrpsysConfigurator.getReferenceRPY(), python.hrpsys_config.HrpsysConfigurator.setSelfGroups(), python.hrpsys_config.HrpsysConfigurator.startAutoBalancer(), python.hrpsys_config.HrpsysConfigurator.startDefaultUnstableControllers(), and python.hrpsys_config.HrpsysConfigurator.stopDefaultUnstableControllers().
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
Referenced by python.hrpsys_config.HrpsysConfigurator.setupLogger().
|
static |
|
static |
python.hrpsys_config.HrpsysConfigurator.kinematics_only_mode |
|
static |
|
static |
|
static |
Referenced by python.hrpsys_config.HrpsysConfigurator.setupLogger().
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
Referenced by python.hrpsys_config.HrpsysConfigurator.setupLogger().
|
static |
|
static |
|
static |
Referenced by python.hrpsys_config.HrpsysConfigurator.checkSimulationMode(), python.hrpsys_config.HrpsysConfigurator.connectComps(), python.hrpsys_config.HrpsysConfigurator.deleteComps(), python.hrpsys_config.HrpsysConfigurator.getRTCInstanceList(), python.hrpsys_config.HrpsysConfigurator.setupLogger(), and python.hrpsys_config.HrpsysConfigurator.waitForRobotHardware().
|
static |
Referenced by python.hrpsys_config.HrpsysConfigurator.calibrateInertiaSensor(), python.hrpsys_config.HrpsysConfigurator.checkEncoders(), python.hrpsys_config.HrpsysConfigurator.checkSimulationMode(), python.hrpsys_config.HrpsysConfigurator.getActualState(), python.hrpsys_config.HrpsysConfigurator.isCalibDone(), python.hrpsys_config.HrpsysConfigurator.lengthDigitalInput(), python.hrpsys_config.HrpsysConfigurator.lengthDigitalOutput(), python.hrpsys_config.HrpsysConfigurator.readDigitalInput(), python.hrpsys_config.HrpsysConfigurator.readDigitalOutput(), python.hrpsys_config.HrpsysConfigurator.removeForceSensorOffset(), python.hrpsys_config.HrpsysConfigurator.servoOff(), python.hrpsys_config.HrpsysConfigurator.servoOn(), python.hrpsys_config.HrpsysConfigurator.setSensorCalibrationJointAngles(), python.hrpsys_config.HrpsysConfigurator.writeDigitalOutput(), and python.hrpsys_config.HrpsysConfigurator.writeDigitalOutputWithMask().
|
static |
|
static |
Referenced by python.hrpsys_config.HrpsysConfigurator.setupLogger().
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
Referenced by python.hrpsys_config.HrpsysConfigurator.connectComps().
|
static |
Referenced by python.hrpsys_config.HrpsysConfigurator.clear(), python.hrpsys_config.HrpsysConfigurator.clearJointAngles(), python.hrpsys_config.HrpsysConfigurator.clearJointAnglesOfGroup(), python.hrpsys_config.HrpsysConfigurator.clearOfGroup(), python.hrpsys_config.HrpsysConfigurator.loadPattern(), python.hrpsys_config.HrpsysConfigurator.playPattern(), python.hrpsys_config.HrpsysConfigurator.playPatternOfGroup(), python.hrpsys_config.HrpsysConfigurator.setInterpolationMode(), python.hrpsys_config.HrpsysConfigurator.setJointAngle(), python.hrpsys_config.HrpsysConfigurator.setJointAngles(), python.hrpsys_config.HrpsysConfigurator.setJointAnglesOfGroup(), python.hrpsys_config.HrpsysConfigurator.setJointAnglesSequence(), python.hrpsys_config.HrpsysConfigurator.setJointAnglesSequenceOfGroup(), python.hrpsys_config.HrpsysConfigurator.setSelfGroups(), python.hrpsys_config.HrpsysConfigurator.setTargetPose(), python.hrpsys_config.HrpsysConfigurator.waitInterpolation(), and python.hrpsys_config.HrpsysConfigurator.waitInterpolationOfGroup().
|
static |
|
static |
|
static |
|
static |
|
static |
Referenced by python.hrpsys_config.HrpsysConfigurator.checkSimulationMode(), python.hrpsys_config.HrpsysConfigurator.connectComps(), python.hrpsys_config.HrpsysConfigurator.isCalibDone(), python.hrpsys_config.HrpsysConfigurator.isServoOn(), python.hrpsys_config.HrpsysConfigurator.readDigitalInput(), python.hrpsys_config.HrpsysConfigurator.servoOff(), python.hrpsys_config.HrpsysConfigurator.setupLogger(), python.hrpsys_config.HrpsysConfigurator.writeDigitalOutput(), and python.hrpsys_config.HrpsysConfigurator.writeDigitalOutputWithMask().
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |