def readCurrentsFromRobot (self, robot, jointNames, torque_constants, first_order_filter = False):
# Input formattting
from dynamic_graph.sot.core.operator import Selec_of_vector
self._current_selec = Selec_of_vector (self.name + "_current_selec")
model = robot.dynamic.model
for jn in jointNames:
jid = model.getJointId (jn)
assert jid < len(model.joints)
jmodel = model.joints[jid]
# TODO there is no value for the 6 first DoF
assert jmodel.idx_v >= 6
self._current_selec.addSelec (jmodel.idx_v-6,jmodel.idx_v-6 + jmodel.nv)
from dynamic_graph.sot.core.operator import Multiply_of_vector
plug (robot.device.currents, self._current_selec.sin)
self._multiply_by_torque_constants = Multiply_of_vector (self.name + "_multiply_by_torque_constants")
self._multiply_by_torque_constants.sin0.value = torque_constants
if first_order_filter:
from agimus_sot.control.controllers import Controller
self._first_order_current_filter = Controller (self.name + "_first_order_current_filter",
(5.,), (5., 1.,), self.dt, [0. for _ in self.desired_torque])
plug (self._current_selec.sout, self._first_order_current_filter.reference)
plug (self._first_order_current_filter.output, self._multiply_by_torque_constants.sin1)
else:
plug (self._current_selec.sout, self._multiply_by_torque_constants.sin1)
plug (self._multiply_by_torque_constants.sout, self.currentTorqueIn)
class MetaTaskJoint(object):
def __init__(self, dyn, joint, name=None):
if name is None:
name = "joint" + str(joint)
self.dyn = dyn
self.name = name
self.joint = joint
self.feature = FeatureGeneric('feature' + name)
self.featureDes = FeatureGeneric('featureDes' + name)
self.gain = GainAdaptive('gain' + name)
self.selec = Selec_of_vector("selec" + name)
self.selec.selec(joint, joint + 1)
plug(dyn.position, self.selec.sin)
plug(self.selec.sout, self.feature.errorIN)
robotDim = len(dyn.position.value)
Id = identity(robotDim)
J = Id[joint:joint + 1]
self.feature.jacobianIN.value = matrixToTuple(J)
self.feature.setReference(self.featureDes.name)
def plugTask(self):
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
@property
def ref(self):
return self.featureDes.errorIN.value
@ref.setter
def ref(self, v):
self.featureDes.errorIN.value = v
def create_floatingBase(robot):
from dynamic_graph.sot.application.state_observation.initializations.hrp2_model_base_flex_estimator_imu_force import FromLocalToGLobalFrame
floatingBase = FromLocalToGLobalFrame(robot.flex_est, "FloatingBase")
plug(robot.ff_locator.freeflyer_aa, floatingBase.sinPos)
from dynamic_graph.sot.core import Selec_of_vector
base_vel_no_flex = Selec_of_vector('base_vel_no_flex')
plug(robot.ff_locator.v, base_vel_no_flex.sin)
base_vel_no_flex.selec(0, 6)
plug(base_vel_no_flex.sout, floatingBase.sinVel)
return floatingBase
def test_balance_ctrl_openhrp(robot,
use_real_vel=True,
use_real_base_state=False,
startSoT=True):
# BUILD THE STANDARD GRAPH
conf = get_sim_conf()
robot = main_v3(robot, startSoT=False, go_half_sitting=False, conf=conf)
# force current measurements to zero
robot.ctrl_manager.i_measured.value = NJ * (0.0, )
robot.current_ctrl.i_measured.value = NJ * (0.0, )
robot.filters.current_filter.x.value = NJ * (0.0, )
# BYPASS TORQUE CONTROLLER
plug(robot.inv_dyn.tau_des, robot.ctrl_manager.ctrl_torque)
# CREATE SIGNALS WITH ROBOT STATE WITH CORRECT SIZE (36)
robot.q = Selec_of_vector("q")
plug(robot.device.robotState, robot.q.sin)
robot.q.selec(0, NJ + 6)
plug(robot.q.sout, robot.pos_ctrl.base6d_encoders)
plug(robot.q.sout, robot.traj_gen.base6d_encoders)
plug(robot.q.sout, robot.estimator_ft.base6d_encoders)
robot.ros = RosPublish('rosPublish')
robot.device.after.addDownsampledSignal('rosPublish.trigger', 1)
# BYPASS JOINT VELOCITY ESTIMATOR
if (use_real_vel):
robot.dq = Selec_of_vector("dq")
plug(robot.device.robotVelocity, robot.dq.sin)
robot.dq.selec(6, NJ + 6)
plug(robot.dq.sout, robot.pos_ctrl.jointsVelocities)
plug(robot.dq.sout, robot.base_estimator.joint_velocities)
plug(robot.device.gyrometer, robot.base_estimator.gyroscope)
# BYPASS BASE ESTIMATOR
robot.v = Selec_of_vector("v")
plug(robot.device.robotVelocity, robot.v.sin)
robot.v.selec(0, NJ + 6)
if (use_real_base_state):
plug(robot.q.sout, robot.inv_dyn.q)
plug(robot.v.sout, robot.inv_dyn.v)
if (startSoT):
start_sot()
# RESET FORCE/TORQUE SENSOR OFFSET
sleep(10 * robot.timeStep)
robot.estimator_ft.setFTsensorOffsets(24 * (0.0, ))
return robot
def readPositionsFromRobot(self, robot, jointNames):
# TODO Compare current position to self.est_theta_closed and
# so as not to overshoot this position.
# Input formattting
from dynamic_graph.sot.core.operator import Selec_of_vector
self._joint_selec = Selec_of_vector(self.name + "_joint_selec")
model = robot.dynamic.model
for jn in jointNames:
jid = model.getJointId(jn)
assert jid < len(model.joints)
jmodel = model.joints[jid]
self._joint_selec.addSelec(jmodel.idx_v, jmodel.idx_v + jmodel.nv)
plug(robot.dynamic.position, self._joint_selec.sin)
self.setCurrentPositionIn(self._joint_selec.sout)
def readTorquesFromRobot (self, robot, jointNames):
# Input formattting
from dynamic_graph.sot.core.operator import Selec_of_vector
self._torque_selec = Selec_of_vector (self.name + "_torque_selec")
model = robot.dynamic.model
for jn in jointNames:
jid = model.getJointId (jn)
assert jid < len(model.joints)
jmodel = model.joints[jid]
# TODO there is no value for the 6 first DoF
assert jmodel.idx_v >= 6
self._torque_selec.addSelec (jmodel.idx_v-6,jmodel.idx_v-6 + jmodel.nv)
plug (robot.device.ptorques, self._torque_selec.sin)
plug (self._torque_selec.sout, self.currentTorqueIn)
def create_device_filters(robot, dt):
robot.pselec = Selec_of_vector("pselec")
robot.pselec.selec(6, 6 + N_JOINTS)
plug(robot.device.state, robot.pselec.sin)
robot.vselec = Selec_of_vector("vselec")
robot.vselec.selec(6, 6 + N_JOINTS)
plug(robot.device.velocity, robot.vselec.sin)
filters = Bunch()
filters.joints_kin = filter_utils.create_chebi1_checby2_series_filter(
"joints_kin", dt, N_JOINTS)
filters.ft_RF_filter = filter_utils.create_butter_lp_filter_Wn_04_N_2(
"ft_RF_filter", dt, 6)
filters.ft_LF_filter = filter_utils.create_butter_lp_filter_Wn_04_N_2(
"ft_LF_filter", dt, 6)
filters.ft_RH_filter = filter_utils.create_butter_lp_filter_Wn_04_N_2(
"ft_RH_filter", dt, 6)
filters.ft_LH_filter = filter_utils.create_butter_lp_filter_Wn_04_N_2(
"ft_LH_filter", dt, 6)
filters.torque_filter = filter_utils.create_chebi1_checby2_series_filter(
"ptorque_filter", dt, N_JOINTS)
filters.acc_filter = filter_utils.create_chebi1_checby2_series_filter(
"acc_filter", dt, 3)
filters.gyro_filter = filter_utils.create_chebi1_checby2_series_filter(
"gyro_filter", dt, 3)
filters.vel_filter = filter_utils.create_butter_lp_filter_Wn_04_N_2(
"vel_filter", dt, N_JOINTS)
# plug(robot.pselec.sout, filters.joints_kin.x)
plug(robot.device.joint_angles, filters.joints_kin.x)
plug(robot.device.forceRLEG, filters.ft_RF_filter.x)
plug(robot.device.forceLLEG, filters.ft_LF_filter.x)
plug(robot.device.forceRARM, filters.ft_RH_filter.x)
plug(robot.device.forceLARM, filters.ft_LH_filter.x)
plug(robot.device.ptorque, filters.torque_filter.x)
plug(robot.vselec.sout, filters.vel_filter.x)
plug(robot.device.accelerometer, filters.acc_filter.x)
plug(robot.device.gyrometer, filters.gyro_filter.x)
return filters
def create_joint_admittance_controller(joint, Kp, dt, robot, filter=False):
controller = SimpleAdmittanceController("jadmctrl")
controller.Kp.value = Kp
robot.stateselec = Selec_of_vector("state_selec")
robot.stateselec.selec(joint + 6, joint + 7)
plug(robot.device.state, robot.stateselec.sin)
plug(robot.stateselec.sout, controller.state)
robot.tauselec = Selec_of_vector("tau_selec")
robot.tauselec.selec(joint, joint + 1)
if filter and hasattr(robot, 'device_filters'):
plug(robot.device_filters.torque_filter.x_filtered, robot.tauselec.sin)
else:
plug(robot.device.ptorque, robot.tauselec.sin)
plug(robot.tauselec.sout, controller.tau)
controller.tauDes.value = [0.0]
controller.init(dt, 1)
controller.setPosition([robot.device.state.value[joint + 6]])
return controller
def __init__(self, dyn, joint, name=None):
if name is None:
name = "joint" + str(joint)
self.dyn = dyn
self.name = name
self.joint = joint
self.feature = FeatureGeneric('feature' + name)
self.featureDes = FeatureGeneric('featureDes' + name)
self.gain = GainAdaptive('gain' + name)
self.selec = Selec_of_vector("selec" + name)
self.selec.selec(joint, joint + 1)
plug(dyn.position, self.selec.sin)
plug(self.selec.sout, self.feature.errorIN)
robotDim = len(dyn.position.value)
Id = identity(robotDim)
J = Id[joint:joint + 1]
self.feature.jacobianIN.value = matrixToTuple(J)
self.feature.setReference(self.featureDes.name)
class AdmittanceControl(object):
"""
The torque controller is then use to maintain a desired force.
It outputs a velocity command to be sent to entity Device.
"""
def __init__(self, name, estimated_theta_closed, desired_torque, period,
nums, denoms):
"""
- estimated_theta_closed: Use for the initial position control. It should correspond to a configuration in collision.
The closer to contact configuration, the least the overshoot.
- desired_torque: The torque to be applied on the object.
- period: The SoT integration time.
- nums, denoms: coefficient of the controller:
\sum_i denoms[i] * d^i theta / dt^i = \sum_j nums[j] d^j torque / dt^j
"""
self.name = name
self.est_theta_closed = estimated_theta_closed
self.desired_torque = desired_torque
self.dt = period
self._makeTorqueControl(nums, denoms)
self._makeIntegrationOfVelocity()
### Feed-forward - contact phase
def _makeTorqueControl(self, nums, denoms):
from agimus_sot.control.controllers import Controller
self.torque_controller = Controller(
self.name + "_torque", nums, denoms, self.dt,
[0. for _ in self.est_theta_closed])
self.torque_controller.addFeedback()
self.torque_controller.reference.value = self.desired_torque
### Internal method
def _makeIntegrationOfVelocity(self):
from dynamic_graph.sot.core import Add_of_vector
self.omega2theta = Add_of_vector(self.name + "_omega2theta")
self.omega2theta.setCoeff2(self.dt)
# self.omega2theta.sin1 <- current position
# self.omega2theta.sin2 <- desired velocity
plug(self.torque_controller.output, self.omega2theta.sin2)
### Setup event to tell when object is grasped and simulate torque feedback.
def setupFeedbackSimulation(self, mass, damping, spring, theta0):
from agimus_sot.control.controllers import Controller
from dynamic_graph.sot.core import Add_of_vector
from agimus_sot.sot import DelayVector
## theta -> phi = theta - theta0
self._sim_theta2phi = Add_of_vector(self.name + "_sim_theta2phi")
self._sim_theta2phi.setCoeff1(1)
self._sim_theta2phi.setCoeff2(-1)
self._sim_theta2phi.sin2.value = theta0
## phi -> torque
from dynamic_graph.sot.core.switch import SwitchVector
from dynamic_graph.sot.core.operator import CompareVector
# reverse = self.theta_open[0] > theta0[0]
reverse = self.desired_torque[0] < 0
self.sim_contact_condition = CompareVector(self.name +
"_sim_contact_condition")
self.sim_contact_condition.setTrueIfAny(False)
self._sim_torque = SwitchVector(self.name + "_sim_torque")
self._sim_torque.setSignalNumber(2)
plug(self.sim_contact_condition.sout, self._sim_torque.boolSelection)
# If phi < 0 (i.e. sim_contact_condition.sout == 1) -> non contact phase
# else contact phase
if reverse:
plug(self._sim_theta2phi.sout, self.sim_contact_condition.sin2)
self.sim_contact_condition.sin1.value = [
0. for _ in self.est_theta_closed
]
else:
plug(self._sim_theta2phi.sout, self.sim_contact_condition.sin1)
self.sim_contact_condition.sin2.value = [
0. for _ in self.est_theta_closed
]
# Non contact phase
# torque = 0, done above
# Contact phase
self.phi2torque = Controller(self.name + "_sim_phi2torque", (
spring,
damping,
mass,
), (1., ), self.dt, [0. for _ in self.est_theta_closed])
#TODO if M != 0: phi2torque.pushNumCoef(((M,),))
plug(self._sim_theta2phi.sout, self.phi2torque.reference)
# Condition
# if phi < 0 -> no contact -> torque = 0
self._sim_torque.sin1.value = [0. for _ in self.est_theta_closed]
# else -> contact -> phi2torque
plug(self.phi2torque.output, self._sim_torque.sin0)
plug(self._sim_torque.sout, self.currentTorqueIn)
def readPositionsFromRobot(self, robot, jointNames):
# TODO Compare current position to self.est_theta_closed and
# so as not to overshoot this position.
# Input formattting
from dynamic_graph.sot.core.operator import Selec_of_vector
self._joint_selec = Selec_of_vector(self.name + "_joint_selec")
model = robot.dynamic.model
for jn in jointNames:
jid = model.getJointId(jn)
assert jid < len(model.joints)
jmodel = model.joints[jid]
self._joint_selec.addSelec(jmodel.idx_v, jmodel.idx_v + jmodel.nv)
plug(robot.dynamic.position, self._joint_selec.sin)
self.setCurrentPositionIn(self._joint_selec.sout)
## - param torque_constants: Should take into account the motor torque constant and the gear ratio.
## - param first_order_filter: Add a first order filter to the current signal.
def readCurrentsFromRobot(self,
robot,
jointNames,
torque_constants,
first_order_filter=False):
# Input formattting
from dynamic_graph.sot.core.operator import Selec_of_vector
self._current_selec = Selec_of_vector(self.name + "_current_selec")
model = robot.dynamic.model
for jn in jointNames:
jid = model.getJointId(jn)
assert jid < len(model.joints)
jmodel = model.joints[jid]
# TODO there is no value for the 6 first DoF
assert jmodel.idx_v >= 6
self._current_selec.addSelec(jmodel.idx_v - 6,
jmodel.idx_v - 6 + jmodel.nv)
from dynamic_graph.sot.core.operator import Multiply_of_vector
plug(robot.device.currents, self._current_selec.sin)
self._multiply_by_torque_constants = Multiply_of_vector(
self.name + "_multiply_by_torque_constants")
self._multiply_by_torque_constants.sin0.value = torque_constants
if first_order_filter:
from agimus_sot.control.controllers import Controller
self._first_order_current_filter = Controller(
self.name + "_first_order_current_filter", (5., ), (
5.,
1.,
), self.dt, [0. for _ in self.desired_torque])
plug(self._current_selec.sout,
self._first_order_current_filter.reference)
plug(self._first_order_current_filter.output,
self._multiply_by_torque_constants.sin1)
else:
plug(self._current_selec.sout,
self._multiply_by_torque_constants.sin1)
plug(self._multiply_by_torque_constants.sout, self.currentTorqueIn)
def readTorquesFromRobot(self, robot, jointNames):
# Input formattting
from dynamic_graph.sot.core.operator import Selec_of_vector
self._torque_selec = Selec_of_vector(self.name + "_torque_selec")
model = robot.dynamic.model
for jn in jointNames:
jid = model.getJointId(jn)
assert jid < len(model.joints)
jmodel = model.joints[jid]
# TODO there is no value for the 6 first DoF
assert jmodel.idx_v >= 6
self._torque_selec.addSelec(jmodel.idx_v - 6,
jmodel.idx_v - 6 + jmodel.nv)
plug(robot.device.ptorques, self._torque_selec.sin)
plug(self._torque_selec.sout, self.currentTorqueIn)
# TODO remove me
def addOutputTo(self, robot, jointNames, mix_of_vector, sot=None):
#TODO assert isinstance(mix_of_vector, Mix_of_vector)
i = mix_of_vector.getSignalNumber()
mix_of_vector.setSignalNumber(i + 1)
plug(self.outputVelocity, mix_of_vector.signal("sin" + str(i)))
model = robot.dynamic.model
for jn in jointNames:
jid = model.getJointId(jn)
jmodel = model.joints[jid]
mix_of_vector.addSelec(i, jmodel.idx_v, jmodel.nv)
def addTracerRealTime(self, robot):
from dynamic_graph.tracer_real_time import TracerRealTime
from agimus_sot.tools import filename_escape
self._tracer = TracerRealTime(self.name + "_tracer")
self._tracer.setBufferSize(10 * 1048576) # 10 Mo
self._tracer.open("/tmp", filename_escape(self.name), ".txt")
self._tracer.add(self.omega2theta.name + ".sin1", "_theta_current")
self._tracer.add(self.omega2theta.name + ".sin2", "_omega_desired")
self._tracer.add(self.omega2theta.name + ".sout", "_theta_desired")
self._tracer.add(self.torque_controller.referenceName,
"_reference_torque")
self._tracer.add(self.torque_controller.measurementName,
"_measured_torque")
if hasattr(self, "_current_selec"):
self._tracer.add(self._current_selec.name + ".sout",
"_measured_current")
robot.device.after.addSignal(self._tracer.name + ".triger")
return self._tracer
@property
def outputPosition(self):
return self.omega2theta.sout
@property
def referenceTorqueIn(self):
return self.torque_controller.reference
def setCurrentPositionIn(self, sig):
plug(sig, self.omega2theta.sin1)
if hasattr(self, "_sim_theta2phi"):
plug(sig, self._sim_theta2phi.sin1)
@property
def currentTorqueIn(self):
return self.torque_controller.measurement
@property
def torqueConstants(self):
return self._multiply_by_torque_constants.sin0
def create_encoders(robot):
encoders = Selec_of_vector('qn')
plug(robot.device.robotState, encoders.sin)
encoders.selec(6, NJ + 6)
return encoders
robot.timeStep = robot.device.getTimeStep()
timeStep = robot.timeStep
RightPitchJoint = 10
RightRollJoint = 11
LeftPitchJoint = 4
LeftRollJoint = 5
Kp = [0.00005]
# --- Ankle admittance foot
# --- RIGHT ANKLE PITCH
controller = SimpleAdmittanceController("rightPitchAnkleController")
controller.Kp.value = Kp
robot.stateselecRP = Selec_of_vector("stateselecRP")
robot.stateselecRP.selec(RightPitchJoint + 6, RightPitchJoint + 7)
plug(robot.device.state, robot.stateselecRP.sin)
plug(robot.stateselecRP.sout, controller.state)
robot.tauselecRP = Selec_of_vector("tauselecRP")
robot.tauselecRP.selec(RightPitchJoint, RightPitchJoint + 1)
plug(robot.device.ptorque, robot.tauselecRP.sin)
plug(robot.tauselecRP.sout, controller.tau)
controller.tauDes.value = [0.0]
controller.init(timeStep, 1)
controller.setPosition([robot.device.state.value[RightPitchJoint + 6]])
robot.rightPitchAnkleController = controller
robot.rightAnklePitchTask = MetaTaskKineJoint(robot.dynamic, RightPitchJoint)
def create_joint_torque_selector(robot, conf):
encoders = Selec_of_vector('selecDdpJointTorque')
plug(robot.device.ptorque, encoders.sin)
encoders.selec(conf.controlled_joint, conf.controlled_joint + 1)
return encoders
def create_joint_vel_selector(robot, conf):
encoders = Selec_of_vector('selecDdpJointVel')
plug(robot.device.robotVelocity, encoders.sin)
encoders.selec(conf.controlled_joint + 6, conf.controlled_joint + 7)
return encoders
def create_simple_graph():
"""
We create a simple graph and create its graphical representation
"""
# here we create some input to graph. These a constant vector but it could
# anything coming from the hardware
centered_slider = VectorConstant("4_robot_sliders")
centered_slider.sout.value = [0.5, 0.5, 0.5, 0.5]
# Filter the centered sliders
# Hence we create a "Finite Impendance Response" filter.
# the filter is in the following form:
# out = sum_{i=0}^{N} data_i * alpha_i
# - the data_i are the collected elements, their number grows until the
# size of the filter is reached.
# - the alpha_i are the gains of the filter, they are defined by the
# method "setElement(index, value)"
# in the end here we do an averaging filter on 200 points.
slider_filtered = FIRFilter_Vector_double("slider_fir_filter")
filter_size = 200
slider_filtered.setSize(filter_size)
for i in range(filter_size):
slider_filtered.setElement(i, 1.0/float(filter_size))
# we plug the centered sliders output to the input of the filter.
plug(centered_slider.sout, slider_filtered.sin)
# Now we want the slider to be in [-qref, qref]
# So we multiply all sliders by a constant which is max_qref.
scaled_slider = Multiply_double_vector("scaled_slider")
scaled_slider.sin1.value = 2.0
plug(slider_filtered.sout, scaled_slider.sin2)
# Now we need to solve the problem that we have 4 sliders for 8 motors.
# Hence we will map each slider value to 2 motors.
state = {}
for i, leg in enumerate(["fr", "hr", "hl", "fl"]):
# first of all we define the references for the hip joint:
state[leg + "_hip_qref"] = Selec_of_vector(leg + "_hip_qref")
state[leg + "_hip_qref"].selec(i, i+1)
plug(scaled_slider.sout, state[leg + "_hip_qref"].sin)
# Then we define the reference for the knee joint. We want the knee to move
# twice as much as the hip and on the opposite direction
state[leg + "_knee_qref"] = Multiply_double_vector(
leg + "_knee_qref")
state[leg + "_knee_qref"].sin1.value = - 2.0
plug(state[leg + "_hip_qref"].sout,
state[leg + "_knee_qref"].sin2)
# now we need to stack the signals 2 by 2:
state[leg + "_qref"] = Stack_of_vector(leg + "_qref")
state[leg + "_qref"].selec1(0, 1)
state[leg + "_qref"].selec2(0, 1)
# first element is the hip
plug(state[leg + "_hip_qref"].sout,
state[leg + "_qref"].sin1)
# second element is the knee
plug(state[leg + "_knee_qref"].sout,
state[leg + "_qref"].sin2)
robot_state_front_legs = Stack_of_vector("front_legs_state")
plug(state["fr_qref"].sout, robot_state_front_legs.sin1)
plug(state["fl_qref"].sout, robot_state_front_legs.sin2)
robot_state_back_legs = Stack_of_vector("hind_legs_state")
plug(state["hr_qref"].sout, robot_state_back_legs.sin1)
plug(state["hl_qref"].sout, robot_state_back_legs.sin2)
robot_state = Stack_of_vector("robot_des_state")
plug(robot_state_front_legs.sout, robot_state.sin1)
plug(robot_state_back_legs.sout, robot_state.sin2)
def create_encoders_velocity(robot):
encoders = Selec_of_vector('dqn')
plug(robot.device.robotVelocity, encoders.sin)
encoders.selec(6, NJ + 6)
return encoders
def create_joint_pos_selector(robot, conf):
encoders = Selec_of_vector('selecDdpJointPos')
plug(robot.device.robotState, encoders.sin)
encoders.selec(conf.controlled_joint + 6, conf.controlled_joint + 7)
return encoders
def create_pos_des_selector(robot, conf):
encoders = Selec_of_vector('selecDdpJointPosDes')
plug(robot.traj_gen.q, encoders.sin)
encoders.selec(conf.controlled_joint, conf.controlled_joint + 1)
return encoders
def create_base_encoders(robot):
base_encoders = Selec_of_vector('base_encoders')
plug(robot.device.robotState, base_encoders.sin)
base_encoders.selec(0, NJ + 6)
return base_encoders
def create_motor_pos_selector(robot, conf):
encoders = Selec_of_vector('selecDdpMotorPos')
plug(robot.device.motor_angles, encoders.sin)
encoders.selec(conf.controlled_joint, conf.controlled_joint + 1)
return encoders
def create_torque_des_selector2(robot, conf):
encoders = Selec_of_vector('selecDdpTorqueDes2')
plug(robot.torque_ctrl.u, encoders.sin)
encoders.selec(31, 32)
return encoders
def create_tau_des_selector(robot, conf):
encoders = Selec_of_vector('selecDdpTauDes')
plug(robot.inv_dyn.tau_des, encoders.sin)
encoders.selec(conf.controlled_joint, conf.controlled_joint + 1)
return encoders
other_dof = identity(robotDim - 6)
jacobian_posture = hstack([first_6, other_dof])
taskPosture.feature.jacobianIN.value = matrixToTuple(jacobian_posture)
taskPosture.feature.setReference(taskPosture.featureDes.name)
taskPosture.add(taskPosture.feature.name)
# Create the sequence player
aSimpleSeqPlay = SimpleSeqPlay('aSimpleSeqPlay')
aSimpleSeqPlay.load(
"/home/ostasse/devel-src/robotpkg-test3/install/share/pyrene-motions/identification/OEM_arms_60s_500Hz")
aSimpleSeqPlay.setTimeToStart(10.0)
plug(aSimpleSeqPlay.posture, taskPosture.featureDes.errorIN)
# Connects the dynamics to the current feature of the posture task
getPostureValue = Selec_of_vector("current_posture")
getPostureValue.selec(6, robotDim)
plug(robot.dynamic.position, getPostureValue.sin)
plug(getPostureValue.sout, taskPosture.feature.errorIN)
plug(getPostureValue.sout, aSimpleSeqPlay.currentPosture)
# Set the gain of the posture task
setGain(taskPosture.gain, (4.9, 0.9, 0.01, 0.9))
plug(taskPosture.gain.gain, taskPosture.controlGain)
plug(taskPosture.error, taskPosture.gain.error)
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control, robot.device.control)
taskPosture.featureDes.errorIN.recompute(0)
def create_balance_controller(robot,
conf,
motor_params,
dt,
robot_name='robot'):
from dynamic_graph.sot.torque_control.inverse_dynamics_balance_controller import InverseDynamicsBalanceController
ctrl = InverseDynamicsBalanceController("invDynBalCtrl")
try:
plug(robot.base_estimator.q, ctrl.q)
plug(robot.base_estimator.v, ctrl.v)
except:
plug(robot.ff_locator.base6dFromFoot_encoders, ctrl.q)
plug(robot.ff_locator.v, ctrl.v)
try:
from dynamic_graph.sot.core import Selec_of_vector
robot.ddq_des = Selec_of_vector('ddq_des')
plug(ctrl.dv_des, robot.ddq_des.sin)
robot.ddq_des.selec(6, NJ + 6)
#plug(robot.ddq_des.sout, robot.estimator_ft.ddqRef);
except:
print(
"WARNING: Could not connect dv_des from BalanceController to ForceTorqueEstimator"
)
#plug(robot.estimator_ft.contactWrenchRightSole, ctrl.wrench_right_foot);
#plug(robot.estimator_ft.contactWrenchLeftSole, ctrl.wrench_left_foot);
plug(robot.device.forceRLEG, ctrl.wrench_right_foot)
# New
plug(robot.device.forceLLEG, ctrl.wrench_left_foot)
# New
plug(ctrl.tau_des, robot.torque_ctrl.jointsTorquesDesired)
#plug(ctrl.dq_admittance, robot.torque_ctrl.dq_des);
# robot.torque_ctrl.dq_des.value = NJ*(0.0,);
#plug(ctrl.tau_des, robot.estimator_ft.tauDes);
plug(ctrl.right_foot_pos, robot.rf_traj_gen.initial_value)
ctrl.rf_ref_pos.value = robot.rf_traj_gen.initial_value.value
ctrl.rf_ref_vel.value = 12 * (0.0, )
ctrl.rf_ref_acc.value = 12 * (0.0, )
# plug(robot.rf_traj_gen.x, ctrl.rf_ref_pos);
# plug(robot.rf_traj_gen.dx, ctrl.rf_ref_vel);
# plug(robot.rf_traj_gen.ddx, ctrl.rf_ref_acc);
plug(ctrl.left_foot_pos, robot.lf_traj_gen.initial_value)
ctrl.lf_ref_pos.value = robot.lf_traj_gen.initial_value.value
ctrl.lf_ref_vel.value = 12 * (0.0, )
ctrl.lf_ref_acc.value = 12 * (0.0, )
# plug(robot.lf_traj_gen.x, ctrl.lf_ref_pos);
# plug(robot.lf_traj_gen.dx, ctrl.lf_ref_vel);
# plug(robot.lf_traj_gen.ddx, ctrl.lf_ref_acc);
plug(ctrl.right_hand_pos, robot.rh_traj_gen.initial_value)
ctrl.rh_ref_pos.value = robot.rh_traj_gen.initial_value.value
ctrl.rh_ref_vel.value = 12 * (0.0, )
ctrl.rh_ref_acc.value = 12 * (0.0, )
# plug(robot.rh_traj_gen.x, ctrl.rh_ref_pos);
# plug(robot.rh_traj_gen.dx, ctrl.rh_ref_vel);
# plug(robot.rh_traj_gen.ddx, ctrl.rh_ref_acc);
plug(ctrl.left_hand_pos, robot.lh_traj_gen.initial_value)
ctrl.lh_ref_pos.value = robot.lh_traj_gen.initial_value.value
ctrl.lh_ref_vel.value = 12 * (0.0, )
ctrl.lh_ref_acc.value = 12 * (0.0, )
# plug(robot.lh_traj_gen.x, ctrl.lh_ref_pos);
# plug(robot.lh_traj_gen.dx, ctrl.lh_ref_vel);
# plug(robot.lh_traj_gen.ddx, ctrl.lh_ref_acc);
ctrl.posture_ref_pos.value = robot.halfSitting[7:]
ctrl.posture_ref_vel.value = 32 * (0.0, )
ctrl.posture_ref_acc.value = 32 * (0.0, )
ctrl.com_ref_pos.value = robot.dynamic.com.value
ctrl.com_ref_vel.value = 3 * (0.0, )
ctrl.com_ref_acc.value = 3 * (0.0, )
ctrl.waist_ref_pos.value = robot.waist_traj_gen.initial_value.value
ctrl.waist_ref_vel.value = 12 * (0.0, )
ctrl.waist_ref_acc.value = 12 * (0.0, )
# plug(robot.traj_gen.q, ctrl.posture_ref_pos);
# plug(robot.traj_gen.dq, ctrl.posture_ref_vel);
# plug(robot.traj_gen.ddq, ctrl.posture_ref_acc);
# plug(robot.com_traj_gen.x, ctrl.com_ref_pos);
# plug(robot.com_traj_gen.dx, ctrl.com_ref_vel);
# plug(robot.com_traj_gen.ddx, ctrl.com_ref_acc);
# plug(robot.waist_traj_gen.x, ctrl.waist_ref_pos);
# plug(robot.waist_traj_gen.dx, ctrl.waist_ref_vel);
# plug(robot.waist_traj_gen.ddx, ctrl.waist_ref_acc);
# plug(robot.rf_force_traj_gen.x, ctrl.f_ref_right_foot);
# plug(robot.lf_force_traj_gen.x, ctrl.f_ref_left_foot);
# rather than giving to the controller the values of gear ratios and rotor inertias
# it is better to compute directly their product in python and pass the result
# to the C++ entity, because otherwise we get a loss of precision
# ctrl.rotor_inertias.value = conf.ROTOR_INERTIAS;
# ctrl.gear_ratios.value = conf.GEAR_RATIOS;
ctrl.rotor_inertias.value = tuple([
g * g * r
for (g,
r) in zip(motor_params.GEAR_RATIOS, motor_params.ROTOR_INERTIAS)
])
ctrl.gear_ratios.value = NJ * (1.0, )
ctrl.contact_normal.value = conf.FOOT_CONTACT_NORMAL
ctrl.contact_points.value = conf.RIGHT_FOOT_CONTACT_POINTS
ctrl.f_min.value = conf.fMin
ctrl.f_max_right_foot.value = conf.fMax
ctrl.f_max_left_foot.value = conf.fMax
ctrl.mu.value = conf.mu[0]
ctrl.weight_contact_forces.value = (1e2, 1e2, 1e0, 1e3, 1e3, 1e3)
ctrl.kp_com.value = 3 * (conf.kp_com, )
ctrl.kd_com.value = 3 * (conf.kd_com, )
ctrl.kp_constraints.value = 6 * (conf.kp_constr, )
ctrl.kd_constraints.value = 6 * (conf.kd_constr, )
ctrl.kp_feet.value = 6 * (conf.kp_feet, )
ctrl.kd_feet.value = 6 * (conf.kd_feet, )
ctrl.kp_hands.value = 6 * (conf.kp_hands, )
ctrl.kd_hands.value = 6 * (conf.kd_hands, )
ctrl.kp_posture.value = conf.kp_posture
ctrl.kd_posture.value = conf.kd_posture
ctrl.kp_pos.value = conf.kp_pos
ctrl.kd_pos.value = conf.kd_pos
ctrl.kp_waist.value = 6 * (conf.kp_waist, )
ctrl.kd_waist.value = 6 * (conf.kd_waist, )
ctrl.w_com.value = conf.w_com
ctrl.w_feet.value = conf.w_feet
ctrl.w_hands.value = conf.w_hands
ctrl.w_forces.value = conf.w_forces
ctrl.w_posture.value = conf.w_posture
ctrl.w_base_orientation.value = conf.w_base_orientation
ctrl.w_torques.value = conf.w_torques
ctrl.init(dt, robot_name)
return ctrl
