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