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 compute_torques(self, Kp, des_pos, Kd, des_vel, des_fff): self.base_pos_xyz = add_vec_vec( selec_vector(self.vicon_base_position, 0, 3, "base_pos"), self.vicon_offset) self.base_vel_xyz = selec_vector(self.vicon_base_velocity, 0, 3, "base_vel") plug(self.base_pos_xyz, self.com_imp_ctrl.position) plug(self.base_vel_xyz, self.com_imp_ctrl.velocity) # plug(self.base_ang_vel_xyz, self.com_imp_ctrl.angvel) plug(Kp, self.com_imp_ctrl.Kp) plug(Kd, self.com_imp_ctrl.Kd) plug(des_pos, self.com_imp_ctrl.des_pos) plug(des_vel, self.com_imp_ctrl.des_vel) plug(des_fff, self.com_imp_ctrl.des_fff) ### mass in all direction (double to vec returns zero) ## TODO : Check if there is dynamicgraph::double self.com_imp_ctrl.mass.value = self.robot_mass self.control_switch_pos = SwitchVector("control_switch_pos") self.control_switch_pos.setSignalNumber( 2) # we want to switch between 2 signals plug(zero_vec(3, "zero_pos"), self.control_switch_pos.sin0) plug(self.com_imp_ctrl.set_pos_bias, self.control_switch_pos.sin1) self.control_switch_pos.selection.value = 0 self.control_switch_vel = SwitchVector("control_switch_vel") self.control_switch_vel.setSignalNumber( 2) # we want to switch between 2 signals plug(zero_vec(3, "zero_vel"), self.control_switch_vel.sin0) plug(self.com_imp_ctrl.set_vel_bias, self.control_switch_vel.sin1) plug(self.control_switch_pos.selection, self.control_switch_vel.selection) self.biased_base_pos_xyz = subtract_vec_vec( self.base_pos_xyz, self.control_switch_pos.sout, "biased_pos") self.biased_base_vel_xyz = subtract_vec_vec( self.base_vel_xyz, self.control_switch_vel.sout, "biased_vel") plug(self.biased_base_pos_xyz, self.com_imp_ctrl.biased_pos) plug(self.biased_base_vel_xyz, self.com_imp_ctrl.biased_vel) self.torques = self.com_imp_ctrl.tau return self.torques
def __init__(self, sotrobot, lpTasks=None, hpTasks=None): self.sotrobot = sotrobot self.hpTasks = hpTasks if hpTasks is not None else _hpTasks(sotrobot) self.lpTasks = lpTasks if lpTasks is not None else _lpTasks(sotrobot) self.currentSot = None from dynamic_graph.sot.core.switch import SwitchVector self.sot_switch = SwitchVector("sot_supervisor_switch") plug(self.sot_switch.sout, self.sotrobot.device.control) from agimus_sot.events import Events self.done_events = Events("done", sotrobot) self.error_events = Events("error", sotrobot) self.done_events.setupNormOfControl(sotrobot.device.control, 1e-2) self.done_events.setupTime( ) # For signal self. done_events.timeEllapsedSignal self.error_events.setupTime( ) # For signal self.error_events.timeEllapsedSignal
def __init__ (self,name,controllers,threshold_up,threshold_down): """ Use controller 0 until the condition signal value becomes greater than threshold. Then use controller 1. Manually switch between controller using the latch. OFF means controller 0 and ON means controller 1. Currently support only two controllers. - controllers: output signal of a controller """ self.reverse = (threshold_up < threshold_down) # any(threshold_up < measured_torque) => torque control self._condition_up = CompareVector (name + "_condition_up") self._condition_up.setTrueIfAny(True) self._condition_up.sin1.value = threshold_up self._condition_up.sin2.value = threshold_up # all(measured_torque < threshold_down) => position control self._condition_down = CompareVector (name + "_condition_down") self._condition_down.setTrueIfAny(False) self._condition_down.sin1.value = threshold_down self._condition_down.sin2.value = threshold_down self._event_up = Event (name + "_event_up") self._event_down = Event (name + "_event_down") self._event_up .setOnlyUp(True); self._event_down .setOnlyUp(True); self._latch = Latch(name + "_latch") self._latch.turnOff() self._switch = SwitchVector (name + "_switch") self._switch.setSignalNumber(len(controllers)) plug(self._condition_up .sout, self._event_up .condition) plug(self._condition_down.sout, self._event_down.condition) plug(self._latch.out , self._switch.boolSelection) # This is necessary to initialize the event (the first recompute triggers the event...) self._event_up.check.recompute(0) self._event_down.check.recompute(0) self._event_up .addSignal (name + "_latch.turnOnSout") self._event_down.addSignal (name + "_latch.turnOffSout") for n, sig in enumerate(controllers): plug(sig, self.signalIn(n))
class Supervisor(object): """ Steps: P = placement, G = grasp, p = pre-P, g = pre-G 0. P <-> GP 1. P <-> gP 2. gP <-> GP 3. GP <-> G 4. GP <-> Gp 5. Gp <-> G """ ## # \param lpTasks list of low priority tasks. If None, a Posture task will be used. # \param hpTasks list of high priority tasks (like balance) def __init__(self, sotrobot, lpTasks=None, hpTasks=None): self.sotrobot = sotrobot self.hpTasks = hpTasks if hpTasks is not None else _hpTasks(sotrobot) self.lpTasks = lpTasks if lpTasks is not None else _lpTasks(sotrobot) self.currentSot = None from dynamic_graph.sot.core.switch import SwitchVector self.sot_switch = SwitchVector("sot_supervisor_switch") plug(self.sot_switch.sout, self.sotrobot.device.control) from agimus_sot.events import Events self.done_events = Events("done", sotrobot) self.error_events = Events("error", sotrobot) self.done_events.setupNormOfControl(sotrobot.device.control, 1e-2) self.done_events.setupTime( ) # For signal self. done_events.timeEllapsedSignal self.error_events.setupTime( ) # For signal self.error_events.timeEllapsedSignal def makeInitialSot(self): # Create the initial sot (keep) from .solver import Solver sot = Solver('sot_keep', self.sotrobot.dynamic.getDimension()) self.keep_posture = Posture("posture_keep", self.sotrobot) self.keep_posture._task.setWithDerivative(False) self.keep_posture._signalPositionRef( ).value = self.sotrobot.dynamic.position.value self.keep_posture.pushTo(sot) sot.doneSignal = self.done_events.controlNormSignal sot.errorSignal = False self.addSolver("", sot) ## Set the robot base pose in the world. # \param basePose a list: [x,y,z,r,p,y] or [x,y,z,qx,qy,qz,qw] # \return success True in case of success def setBasePose(self, basePose): if len(basePose) == 7: # Currently, this case never happens from dynamic_graph.sot.tools.quaternion import Quaternion from numpy.linalg import norm q = Quaternion(basePose[6], basePose[3], basePose[4], basePose[5]) if abs(norm(q.array) - 1.) > 1e-2: return False, "Quaternion is not normalized" basePose = basePose[:3] + q.toRPY().tolist() if self.currentSot == "" or len(basePose) != 6: # We are using the SOT to keep the current posture. # The 6 first DoF are not used by the task so we can change them safely. self.sotrobot.device.set( tuple(basePose + list(self.sotrobot.device.state.value[6:]))) self.keep_posture._signalPositionRef( ).value = self.sotrobot.device.state.value return True else: return False ## \name SoT managements ## \{ def addPreAction(self, name, preActionSolver): self.preActions[name] = preActionSolver self._addSignalToSotSwitch(preActionSolver) def addSolver(self, name, solver): self.sots[name] = solver self._addSignalToSotSwitch(solver) def duplicateSolver(self, existingSolver, newSolver): self.sots[newSolver] = self.sots[existingSolver] def addPostActions(self, name, postActionSolvers): self.postActions[name] = postActionSolvers for targetState, pa_sot in postActionSolvers.iteritems(): self._addSignalToSotSwitch(pa_sot) ## This is for internal purpose def _addSignalToSotSwitch(self, solver): n = self.sot_switch.getSignalNumber() self.sot_switch.setSignalNumber(n + 1) self.sots_indexes[solver.name] = n plug(solver.control, self.sot_switch.signal("sin" + str(n))) def _plug(e, events, n, name): assert events.getSignalNumber() == n, "Wrong number of events." events.setSignalNumber(n + 1) events.setConditionString(n, name) if isinstance(e, (bool, int)): events.conditionSignal(n).value = int(e) else: plug(e, events.conditionSignal(n)) _plug(solver.doneSignal, self.done_events, n, solver.name) _plug(solver.errorSignal, self.error_events, n, solver.name) def _selectSolver(self, solver): n = self.sots_indexes[solver.name] self.sot_switch.selection.value = n self.done_events.setSelectedSignal(n) self.error_events.setSelectedSignal(n) ## \} def topics(self): c = self.hpTasks + self.lpTasks for g in self.grasps.values(): c += g for p in self.placements.values(): c += p return c.topics def plugTopicsToRos(self): from dynamic_graph.ros.ros_queued_subscribe import RosQueuedSubscribe self.rosSubscribe = RosQueuedSubscribe('ros_queued_subscribe') from dynamic_graph.ros.ros_tf_listener import RosTfListener self.rosTf = RosTfListener('ros_tf_listener') topics = self.topics() for name, topic_info in topics.items(): topic_handler = _handlers[topic_info.get("handler", "default")] topic_handler(name, topic_info, self.rosSubscribe, self.rosTf) def printQueueSize(self): exec("tmp = " + self.rosSubscribe.list()) for l in tmp: print(l, self.rosSubscribe.queueSize(l)) ## Check consistency between two SoTs. # # This is not used anymore because it must be synchronized with the real-time thread. # \todo Re-enable consistency check between two SoTs. def isSotConsistentWithCurrent(self, transitionName, thr=1e-3): if self.currentSot is None or transitionName == self.currentSot: return True csot = self.sots[self.currentSot] nsot = self.sots[transitionName] t = self.sotrobot.device.control.time # This is not safe since it would be run concurrently with the # real time thread. csot.control.recompute(t) nsot.control.recompute(t) from numpy import array, linalg error = array(nsot.control.value) - array(csot.control.value) n = linalg.norm(error) if n > thr: print("Control not consistent:", linalg.norm(error), '\n', error) return False return True def clearQueues(self): self.rosSubscribe.readQueue(-1) exec("tmp = " + self.rosSubscribe.list()) for s in tmp: print('{} queue size: {}'.format(s, self.rosSubscribe.queueSize(s))) self.rosSubscribe.clearQueue(s) ## Wait for the queue to be of a given size. # \param minQueueSize (integer) waits to the queue size of rosSubscribe # to be greater or equal to \c minQueueSize # \param timeout time in seconds after which to return a failure. # \return True on success, False on timeout. def waitForQueue(self, minQueueSize, timeout): ts = self.sotrobot.device.getTimeStep() to = int(timeout / self.sotrobot.device.getTimeStep()) from time import sleep start_it = self.sotrobot.device.control.time exec("queues = " + self.rosSubscribe.list()) for queue in queues: while self.rosSubscribe.queueSize(queue) < minQueueSize: if self.sotrobot.device.control.time > start_it + to: return False, "Queue {} has received {} points.".format( queue, self.rosSubscribe.queueSize(queue)) sleep(ts) return True, "" ## Start reading values received by the RosQueuedSubscribe entity. # \param delay (integer) how many periods to wait before reading. # It allows to give some delay to network connection. # \param minQueueSize (integer) waits to the queue size of rosSubscribe # to be greater or equal to \p minQueueSize # \param duration expected duration (in seconds) of the queue. # \param timeout time in seconds after which to return a failure. # \return success, time boolean, SoT time at which reading starts (invalid if success is False) # # \warning If \p minQueueSize is greater than the number of values to # be received by rosSubscribe, this function does an infinite loop. def readQueue(self, delay, minQueueSize, duration, timeout): from time import sleep print("Current solver {0}".format(self.currentSot)) if delay < 0: print("Delay argument should be >= 0") return False, -1 minSizeReached, msg = self.waitForQueue(minQueueSize, timeout) if not minSizeReached: return False, -1 durationStep = int(duration / self.sotrobot.device.getTimeStep()) t = self.sotrobot.device.control.time + delay self.rosSubscribe.readQueue(t) self.done_events.setFutureTime(t + durationStep) self.error_events.setFutureTime(t + durationStep) return True, t def stopReadingQueue(self): self.rosSubscribe.readQueue(-1) # \return success, time boolean, SoT time at which reading starts (invalid if success is False) def plugSot(self, transitionName, check=False): if check and not self.isSotConsistentWithCurrent(transitionName): # raise Exception ("Sot %d not consistent with sot %d" % (self.currentSot, id)) print("Sot {0} not consistent with sot {1}".format( self.currentSot, transitionName)) if transitionName == "": self.keep_posture._signalPositionRef( ).value = self.sotrobot.dynamic.position.value solver = self.sots[transitionName] # No done events should be triggered before call # to readQueue. We expect it to happen with 1e6 milli-seconds # from now... devicetime = self.sotrobot.device.control.time self.done_events.setFutureTime(devicetime + 100000) self._selectSolver(solver) print("{0}: Current solver {1}\n{2}".format(devicetime, transitionName, solver.sot.display())) self.currentSot = transitionName if hasattr(self, 'ros_publish_state'): self.ros_publish_state.transition_name.value = transitionName return True, devicetime # \return success, time boolean, SoT time at which reading starts (invalid if success is False) def runPreAction(self, transitionName): if self.preActions.has_key(transitionName): solver = self.preActions[transitionName] t = self.sotrobot.device.control.time + 2 self.done_events.setFutureTime(t) self._selectSolver(solver) print("{0}: Running pre action {1}\n{2}".format( t, transitionName, solver.sot.display())) return True, t - 2 print("No pre action", transitionName) return False, -1 ## Execute a post-action # \return success, time boolean, SoT time at which reading starts (invalid if success is False) def runPostAction(self, targetStateName): if self.postActions.has_key(self.currentSot): d = self.postActions[self.currentSot] if d.has_key(targetStateName): solver = d[targetStateName] devicetime = self.sotrobot.device.control.time self.done_events.setFutureTime(devicetime + 2) self._selectSolver(solver) print("{0}: Running post action {1} --> {2}\n{3}".format( devicetime, self.currentSot, targetStateName, solver.sot.display())) return True, devicetime print("No post action {0} --> {1}".format(self.currentSot, targetStateName)) return False, -1 def getJointList(self, prefix=""): return [prefix + n for n in self.sotrobot.dynamic.model.names[1:]] def publishState(self, subsampling=40): if hasattr(self, "ros_publish_state"): return from dynamic_graph.ros import RosPublish self.ros_publish_state = RosPublish("ros_publish_state") self.ros_publish_state.add("vector", "state", "/agimus/sot/state") self.ros_publish_state.add("vector", "reference_state", "/agimus/sot/reference_state") self.ros_publish_state.add("string", "transition_name", "/agimus/sot/transition_name") self.ros_publish_state.transition_name.value = "" plug(self.sotrobot.device.state, self.ros_publish_state.state) plug(self.rosSubscribe.posture, self.ros_publish_state.reference_state) self.sotrobot.device.after.addDownsampledSignal( "ros_publish_state.trigger", subsampling)
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 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 ## omega -> theta # Done in _makeControllerSwich # A delay is necessary to avoid loops in SoT delayTheta = DelayVector(self.name + "_sim_theta_delay") delayTheta.setMemory(tuple([0. for _ in self.est_theta_closed])) plug(self.omega2theta.sout, delayTheta.sin) self.setCurrentPositionIn(delayTheta.previous) ## theta -> phi = theta - theta0 self.theta2phi = Add_of_vector(self.name + "_sim_theta2phi") self.theta2phi.setCoeff1(1) self.theta2phi.setCoeff2(-1) plug(delayTheta.current, self.theta2phi.sin1) self.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_switch = SwitchVector(self.name + "_sim_torque") self.sim_switch.setSignalNumber(2) plug(self.sim_contact_condition.sout, self.sim_switch.boolSelection) # Non contact phase if reverse: plug(self.theta2phi.sout, self.sim_contact_condition.sin2) self.sim_contact_condition.sin1.value = [ 0. for _ in self.est_theta_closed ] else: plug(self.theta2phi.sout, self.sim_contact_condition.sin1) self.sim_contact_condition.sin2.value = [ 0. for _ in self.est_theta_closed ] # 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.theta2phi.sout, self.phi2torque.reference) # Condition # if phi < 0 -> no contact -> torque = 0 self.sim_switch.sin1.value = [0. for _ in self.est_theta_closed] # else -> contact -> phi2torque plug(self.phi2torque.output, self.sim_switch.sin0) delay = DelayVector(self.name + "_sim_torque_delay") delay.setMemory(tuple([0. for _ in self.est_theta_closed])) # plug (self.phi2torque.output, delay.sin) plug(self.sim_switch.sout, delay.sin) # self.setCurrentConditionIn (delay.current) plug(delay.previous, self.currentTorqueIn)
def return_end_eff_lqr_tau(self, des_pos, des_vel, des_ori, des_ang_vel, des_fff, des_lqr): """ ### for lqr based controller at the end effector. reads values obtained from the planner """ self.base_pos_xyz = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_position"), 0, 3, "base_pos") self.base_vel_xyz = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_velocity_body"), 0, 3, "base_vel") plug(self.base_pos_xyz, self.com_imp_ctrl.position) plug(self.base_vel_xyz, self.com_imp_ctrl.velocity) self.control_switch_pos = SwitchVector("control_switch_pos") self.control_switch_pos.setSignalNumber( 2) # we want to switch between 2 signals plug(zero_vec(3, "zero_pos"), self.control_switch_pos.sin0) plug(self.com_imp_ctrl.set_pos_bias, self.control_switch_pos.sin1) self.control_switch_pos.selection.value = 0 self.control_switch_vel = SwitchVector("control_switch_vel") self.control_switch_vel.setSignalNumber( 2) # we want to switch between 2 signals plug(zero_vec(3, "zero_vel"), self.control_switch_vel.sin0) plug(self.com_imp_ctrl.set_vel_bias, self.control_switch_vel.sin1) plug(self.control_switch_pos.selection, self.control_switch_vel.selection) self.base_orientation = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_position"), 3, 7, "base_orientation") self.biased_base_pos_xyz = subtract_vec_vec( self.base_pos_xyz, self.control_switch_pos.sout, "biased_pos") self.biased_base_vel_xyz = subtract_vec_vec( self.base_vel_xyz, self.control_switch_vel.sout, "biased_vel") self.base_ang_vel_xyz = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_velocity_body"), 3, 6, "selec_ang_dxyz") #### LQR equation: ###should be removed plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kp) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kd) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kp_ang) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kd_ang) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.inertia) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.mass) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.des_fft) ### plug(des_lqr, self.com_imp_ctrl.lqr_gain) plug(self.biased_base_pos_xyz, self.com_imp_ctrl.biased_pos) plug(des_pos, self.com_imp_ctrl.des_pos) plug(self.biased_base_vel_xyz, self.com_imp_ctrl.biased_vel) plug(des_vel, self.com_imp_ctrl.des_vel) plug(des_fff, self.com_imp_ctrl.des_fff) plug(self.base_orientation, self.com_imp_ctrl.ori) plug(self.base_ang_vel_xyz, self.com_imp_ctrl.angvel) plug(des_ori, self.com_imp_ctrl.des_ori) plug(des_ang_vel, self.com_imp_ctrl.des_ang_vel) lqr_end_eff_force = self.com_imp_ctrl.end_eff_lqr_tau torques_fl = selec_vector(lqr_end_eff_force, 0, 3, self.EntityName + "torques_fl") torques_fr = selec_vector(lqr_end_eff_force, 3, 6, self.EntityName + "torques_fr") torques_hl = selec_vector(lqr_end_eff_force, 6, 9, self.EntityName + "torques_hl") torques_hr = selec_vector(lqr_end_eff_force, 9, 12, self.EntityName + "torques_hr") torques_fl_6d = stack_two_vectors(torques_fl, zero_vec(3, "stack_fl_tau"), 3, 3) torques_fr_6d = stack_two_vectors(torques_fr, zero_vec(3, "stack_fr_tau"), 3, 3) torques_hl_6d = stack_two_vectors(torques_hl, zero_vec(3, "stack_hl_tau"), 3, 3) torques_hr_6d = stack_two_vectors(torques_hr, zero_vec(3, "stack_hr_tau"), 3, 3) torques_fl_fr = stack_two_vectors(torques_fl_6d, torques_fr_6d, 6, 6) torques_hl_hr = stack_two_vectors(torques_hl_6d, torques_hr_6d, 6, 6) lqr_end_eff_force_24d = stack_two_vectors(torques_fl_fr, torques_hl_hr, 12, 12) lqr_end_eff_force_24d = mul_double_vec(1.0, lqr_end_eff_force_24d, "lqr_end_eff_force") return lqr_end_eff_force_24d
def return_lqr_tau(self, des_pos, des_vel, des_ori, des_ang_vel, des_fff, des_fft, des_lqr): """ ### for lqr based controller. reads values obtained from the planner """ self.base_pos_xyz = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_position"), 0, 3, "base_pos") self.base_vel_xyz = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_velocity_body"), 0, 3, "base_vel") plug(self.base_pos_xyz, self.com_imp_ctrl.position) plug(self.base_vel_xyz, self.com_imp_ctrl.velocity) self.control_switch_pos = SwitchVector("control_switch_pos") self.control_switch_pos.setSignalNumber( 2) # we want to switch between 2 signals plug(zero_vec(3, "zero_pos"), self.control_switch_pos.sin0) plug(self.com_imp_ctrl.set_pos_bias, self.control_switch_pos.sin1) self.control_switch_pos.selection.value = 0 self.control_switch_vel = SwitchVector("control_switch_vel") self.control_switch_vel.setSignalNumber( 2) # we want to switch between 2 signals plug(zero_vec(3, "zero_vel"), self.control_switch_vel.sin0) plug(self.com_imp_ctrl.set_vel_bias, self.control_switch_vel.sin1) plug(self.control_switch_pos.selection, self.control_switch_vel.selection) self.base_orientation = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_position"), 3, 7, "base_orientation") self.biased_base_pos_xyz = subtract_vec_vec( self.base_pos_xyz, self.control_switch_pos.sout, "biased_pos") self.biased_base_vel_xyz = subtract_vec_vec( self.base_vel_xyz, self.control_switch_vel.sout, "biased_vel") self.base_ang_vel_xyz = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_velocity_body"), 3, 6, "selec_ang_dxyz") #### LQR equation: ###should be removed plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kp) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kd) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kp_ang) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kd_ang) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.inertia) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.mass) ### plug(des_lqr, self.com_imp_ctrl.lqr_gain) plug(self.biased_base_pos_xyz, self.com_imp_ctrl.biased_pos) plug(des_pos, self.com_imp_ctrl.des_pos) plug(self.biased_base_vel_xyz, self.com_imp_ctrl.biased_vel) plug(des_vel, self.com_imp_ctrl.des_vel) plug(des_fff, self.com_imp_ctrl.des_fff) plug(self.base_orientation, self.com_imp_ctrl.ori) plug(self.base_ang_vel_xyz, self.com_imp_ctrl.angvel) plug(des_ori, self.com_imp_ctrl.des_ori) plug(des_ang_vel, self.com_imp_ctrl.des_ang_vel) plug(des_fft, self.com_imp_ctrl.des_fft) lqr_com_force = self.com_imp_ctrl.lqrtau return lqr_com_force
class QuadrupedComControl(object): def __init__(self, robot, ViconClientEntity=None, client_name="vicon_client", vicon_ip='10.32.3.16:801', EntityName="quad_com_ctrl", base_position=None, base_velocity=None): """ Args: base_position: (Optional, Vec7d signal) Base position of the robot. base_velocity: (Optional, Vec6d signal) Base velocity of the robot. """ self.robot = robot self.EntityName = EntityName self.init_robot_properties() self.client_name = client_name self.host_name_quadruped = vicon_ip if ViconClientEntity: self.vicon_client = ViconClientEntity('vicon_' + self.client_name) self.vicon_client.connect_to_vicon(self.host_name_quadruped) self.vicon_client.displaySignals() self.vicon_client.add_object_to_track("{}/{}".format( self.robot_vicon_name, self.robot_vicon_name)) try: ## comment out if running on real robot self.vicon_client.robot_wrapper(robot, self.robot_vicon_name) except: print("not in simulation") self.robot.add_trace(self.vicon_client.name, self.robot_vicon_name + "_position") self.robot.add_trace(self.vicon_client.name, self.robot_vicon_name + "_velocity_body") self.robot.add_trace(self.vicon_client.name, self.robot_vicon_name + "_velocity_world") self.vicon_base_position = self.vicon_client.signal( self.robot_vicon_name + "_position") self.vicon_base_velocity = self.vicon_client.signal( self.robot_vicon_name + "_velocity_body") elif base_position and base_velocity: self.vicon_base_position = base_position self.vicon_base_velocity = base_velocity else: raise ValueError( 'Need to provide either ViconClientEntity or (base_positon and base_velocity)' ) self.com_imp_ctrl = ComImpedanceControl(EntityName) self._biased_base_position = None self._biased_base_velocity = None self.vicon_offset = constVector([ 0., 0., 0., ]) def init_robot_properties(self): self.robot_vicon_name = "quadruped" self.robot_mass = [2.17784, 2.17784, 2.17784] self.robot_base_inertia = [0.00578574, 0.01938108, 0.02476124] def compute_torques(self, Kp, des_pos, Kd, des_vel, des_fff): self.base_pos_xyz = add_vec_vec( selec_vector(self.vicon_base_position, 0, 3, "base_pos"), self.vicon_offset) self.base_vel_xyz = selec_vector(self.vicon_base_velocity, 0, 3, "base_vel") plug(self.base_pos_xyz, self.com_imp_ctrl.position) plug(self.base_vel_xyz, self.com_imp_ctrl.velocity) # plug(self.base_ang_vel_xyz, self.com_imp_ctrl.angvel) plug(Kp, self.com_imp_ctrl.Kp) plug(Kd, self.com_imp_ctrl.Kd) plug(des_pos, self.com_imp_ctrl.des_pos) plug(des_vel, self.com_imp_ctrl.des_vel) plug(des_fff, self.com_imp_ctrl.des_fff) ### mass in all direction (double to vec returns zero) ## TODO : Check if there is dynamicgraph::double self.com_imp_ctrl.mass.value = self.robot_mass self.control_switch_pos = SwitchVector("control_switch_pos") self.control_switch_pos.setSignalNumber( 2) # we want to switch between 2 signals plug(zero_vec(3, "zero_pos"), self.control_switch_pos.sin0) plug(self.com_imp_ctrl.set_pos_bias, self.control_switch_pos.sin1) self.control_switch_pos.selection.value = 0 self.control_switch_vel = SwitchVector("control_switch_vel") self.control_switch_vel.setSignalNumber( 2) # we want to switch between 2 signals plug(zero_vec(3, "zero_vel"), self.control_switch_vel.sin0) plug(self.com_imp_ctrl.set_vel_bias, self.control_switch_vel.sin1) plug(self.control_switch_pos.selection, self.control_switch_vel.selection) self.biased_base_pos_xyz = subtract_vec_vec( self.base_pos_xyz, self.control_switch_pos.sout, "biased_pos") self.biased_base_vel_xyz = subtract_vec_vec( self.base_vel_xyz, self.control_switch_vel.sout, "biased_vel") plug(self.biased_base_pos_xyz, self.com_imp_ctrl.biased_pos) plug(self.biased_base_vel_xyz, self.com_imp_ctrl.biased_vel) self.torques = self.com_imp_ctrl.tau return self.torques def compute_ang_control_torques(self, Kp_ang, des_ori, Kd_ang, des_ang_vel, des_fft): """ ### Computes torques required to control the orientation of base """ self.base_orientation = selec_vector(self.vicon_base_position, 3, 7, "base_orientation") self.base_ang_vel_xyz = selec_vector(self.vicon_base_velocity, 3, 6, "selec_ang_dxyz") plug(Kp_ang, self.com_imp_ctrl.Kp_ang) plug(Kd_ang, self.com_imp_ctrl.Kd_ang) self.com_imp_ctrl.inertia.value = [0.00578574, 0.01938108, 0.02476124] plug(self.base_orientation, self.com_imp_ctrl.ori) plug(self.base_ang_vel_xyz, self.com_imp_ctrl.angvel) plug(des_ori, self.com_imp_ctrl.des_ori) plug(des_ang_vel, self.com_imp_ctrl.des_ang_vel) plug(des_fft, self.com_imp_ctrl.des_fft) return self.com_imp_ctrl.angtau def set_bias(self): self.control_switch_pos.selection.value = 1 def get_biased_base_position(self): """ Return the robot position taking the bias offset into account. Returns: Signal<dg::vector> of size 7 (0:3 translation, 3:7 quaternion orientation) """ if self._biased_base_position is None: self._biased_base_position = stack_two_vectors( self.biased_base_pos_xyz, self.base_orientation, 3, 4, self.EntityName + '_biased_base_pos') return self._biased_base_position def get_biased_base_velocity(self): """ Return the robot velocity taking the bias offset into account. Returns: Signal<dg::vector> of size 6 (0:3 translation, 3:6 rpy orientation) """ if self._biased_base_velocity is None: self._biased_base_velocity = stack_two_vectors( self.biased_base_vel_xyz, self.base_ang_vel_xyz, 3, 3, self.EntityName + '_biased_base_vel') return self._biased_base_velocity def set_abs_end_eff_pos(self, abs_end_eff_pos_sig): plug(abs_end_eff_pos_sig, self.com_imp_ctrl.abs_end_eff_pos) def threshold_cnt_sensor(self): plug(self.robot.device.contact_sensors, self.com_imp_ctrl.cnt_sensor) return self.com_imp_ctrl.thr_cnt_sensor def convert_cnt_value_to_3d(self, cnt_sensor, start_index, end_index, entityName): ## Converts each element of contact sensor to 3d to allow vec_vec multiplication selec_cnt_value = selec_vector(cnt_sensor, start_index, end_index, entityName) cnt_value_2d = stack_two_vectors(selec_cnt_value, selec_cnt_value, 1, 1) cnt_value_3d = stack_two_vectors(cnt_value_2d, selec_cnt_value, 2, 1) return cnt_value_3d def return_lqr_tau(self, des_pos, des_vel, des_ori, des_ang_vel, des_fff, des_fft, des_lqr): """ ### for lqr based controller. reads values obtained from the planner """ self.base_pos_xyz = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_position"), 0, 3, "base_pos") self.base_vel_xyz = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_velocity_body"), 0, 3, "base_vel") plug(self.base_pos_xyz, self.com_imp_ctrl.position) plug(self.base_vel_xyz, self.com_imp_ctrl.velocity) self.control_switch_pos = SwitchVector("control_switch_pos") self.control_switch_pos.setSignalNumber( 2) # we want to switch between 2 signals plug(zero_vec(3, "zero_pos"), self.control_switch_pos.sin0) plug(self.com_imp_ctrl.set_pos_bias, self.control_switch_pos.sin1) self.control_switch_pos.selection.value = 0 self.control_switch_vel = SwitchVector("control_switch_vel") self.control_switch_vel.setSignalNumber( 2) # we want to switch between 2 signals plug(zero_vec(3, "zero_vel"), self.control_switch_vel.sin0) plug(self.com_imp_ctrl.set_vel_bias, self.control_switch_vel.sin1) plug(self.control_switch_pos.selection, self.control_switch_vel.selection) self.base_orientation = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_position"), 3, 7, "base_orientation") self.biased_base_pos_xyz = subtract_vec_vec( self.base_pos_xyz, self.control_switch_pos.sout, "biased_pos") self.biased_base_vel_xyz = subtract_vec_vec( self.base_vel_xyz, self.control_switch_vel.sout, "biased_vel") self.base_ang_vel_xyz = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_velocity_body"), 3, 6, "selec_ang_dxyz") #### LQR equation: ###should be removed plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kp) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kd) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kp_ang) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kd_ang) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.inertia) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.mass) ### plug(des_lqr, self.com_imp_ctrl.lqr_gain) plug(self.biased_base_pos_xyz, self.com_imp_ctrl.biased_pos) plug(des_pos, self.com_imp_ctrl.des_pos) plug(self.biased_base_vel_xyz, self.com_imp_ctrl.biased_vel) plug(des_vel, self.com_imp_ctrl.des_vel) plug(des_fff, self.com_imp_ctrl.des_fff) plug(self.base_orientation, self.com_imp_ctrl.ori) plug(self.base_ang_vel_xyz, self.com_imp_ctrl.angvel) plug(des_ori, self.com_imp_ctrl.des_ori) plug(des_ang_vel, self.com_imp_ctrl.des_ang_vel) plug(des_fft, self.com_imp_ctrl.des_fft) lqr_com_force = self.com_imp_ctrl.lqrtau return lqr_com_force def return_end_eff_lqr_tau(self, des_pos, des_vel, des_ori, des_ang_vel, des_fff, des_lqr): """ ### for lqr based controller at the end effector. reads values obtained from the planner """ self.base_pos_xyz = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_position"), 0, 3, "base_pos") self.base_vel_xyz = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_velocity_body"), 0, 3, "base_vel") plug(self.base_pos_xyz, self.com_imp_ctrl.position) plug(self.base_vel_xyz, self.com_imp_ctrl.velocity) self.control_switch_pos = SwitchVector("control_switch_pos") self.control_switch_pos.setSignalNumber( 2) # we want to switch between 2 signals plug(zero_vec(3, "zero_pos"), self.control_switch_pos.sin0) plug(self.com_imp_ctrl.set_pos_bias, self.control_switch_pos.sin1) self.control_switch_pos.selection.value = 0 self.control_switch_vel = SwitchVector("control_switch_vel") self.control_switch_vel.setSignalNumber( 2) # we want to switch between 2 signals plug(zero_vec(3, "zero_vel"), self.control_switch_vel.sin0) plug(self.com_imp_ctrl.set_vel_bias, self.control_switch_vel.sin1) plug(self.control_switch_pos.selection, self.control_switch_vel.selection) self.base_orientation = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_position"), 3, 7, "base_orientation") self.biased_base_pos_xyz = subtract_vec_vec( self.base_pos_xyz, self.control_switch_pos.sout, "biased_pos") self.biased_base_vel_xyz = subtract_vec_vec( self.base_vel_xyz, self.control_switch_vel.sout, "biased_vel") self.base_ang_vel_xyz = selec_vector( self.vicon_client.signal(self.robot_vicon_name + "_velocity_body"), 3, 6, "selec_ang_dxyz") #### LQR equation: ###should be removed plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kp) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kd) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kp_ang) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kd_ang) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.inertia) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.mass) plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.des_fft) ### plug(des_lqr, self.com_imp_ctrl.lqr_gain) plug(self.biased_base_pos_xyz, self.com_imp_ctrl.biased_pos) plug(des_pos, self.com_imp_ctrl.des_pos) plug(self.biased_base_vel_xyz, self.com_imp_ctrl.biased_vel) plug(des_vel, self.com_imp_ctrl.des_vel) plug(des_fff, self.com_imp_ctrl.des_fff) plug(self.base_orientation, self.com_imp_ctrl.ori) plug(self.base_ang_vel_xyz, self.com_imp_ctrl.angvel) plug(des_ori, self.com_imp_ctrl.des_ori) plug(des_ang_vel, self.com_imp_ctrl.des_ang_vel) lqr_end_eff_force = self.com_imp_ctrl.end_eff_lqr_tau torques_fl = selec_vector(lqr_end_eff_force, 0, 3, self.EntityName + "torques_fl") torques_fr = selec_vector(lqr_end_eff_force, 3, 6, self.EntityName + "torques_fr") torques_hl = selec_vector(lqr_end_eff_force, 6, 9, self.EntityName + "torques_hl") torques_hr = selec_vector(lqr_end_eff_force, 9, 12, self.EntityName + "torques_hr") torques_fl_6d = stack_two_vectors(torques_fl, zero_vec(3, "stack_fl_tau"), 3, 3) torques_fr_6d = stack_two_vectors(torques_fr, zero_vec(3, "stack_fr_tau"), 3, 3) torques_hl_6d = stack_two_vectors(torques_hl, zero_vec(3, "stack_hl_tau"), 3, 3) torques_hr_6d = stack_two_vectors(torques_hr, zero_vec(3, "stack_hr_tau"), 3, 3) torques_fl_fr = stack_two_vectors(torques_fl_6d, torques_fr_6d, 6, 6) torques_hl_hr = stack_two_vectors(torques_hl_6d, torques_hr_6d, 6, 6) lqr_end_eff_force_24d = stack_two_vectors(torques_fl_fr, torques_hl_hr, 12, 12) lqr_end_eff_force_24d = mul_double_vec(1.0, lqr_end_eff_force_24d, "lqr_end_eff_force") return lqr_end_eff_force_24d def return_com_torques(self, com_tau, ang_tau, des_abs_vel, hess, g0, ce, ci, ci0, reg, cnt_plan=None): ### This divides forces using the wbc controller plug(com_tau, self.com_imp_ctrl.lctrl) plug(ang_tau, self.com_imp_ctrl.actrl) plug(des_abs_vel, self.com_imp_ctrl.abs_end_eff_vel) plug(hess, self.com_imp_ctrl.hess) plug(g0, self.com_imp_ctrl.g0) plug(ce, self.com_imp_ctrl.ce) plug(ci, self.com_imp_ctrl.ci) plug(ci0, self.com_imp_ctrl.ci0) plug(reg, self.com_imp_ctrl.reg) if cnt_plan == None: thr_cnt_sensor = self.threshold_cnt_sensor() plug(thr_cnt_sensor, self.com_imp_ctrl.thr_cnt_value) else: plug(cnt_plan, self.com_imp_ctrl.cnt_sensor) plug(cnt_plan, self.com_imp_ctrl.thr_cnt_value) self.wb_ctrl = self.com_imp_ctrl.wbctrl ## Thresholding with contact sensor to make forces event based # fl_cnt_value = self.convert_cnt_value_to_3d(thr_cnt_sensor, 0, 1, "fl_cnt_3d") # fr_cnt_value = self.convert_cnt_value_to_3d(thr_cnt_sensor, 1, 2, "fr_cnt_3d") # hl_cnt_value = self.convert_cnt_value_to_3d(thr_cnt_sensor, 2, 3, "hl_cnt_3d") # hr_cnt_value = self.convert_cnt_value_to_3d(thr_cnt_sensor, 3, 4, "hr_cnt_3d") torques_fl = selec_vector(self.wb_ctrl, 0, 3, self.EntityName + "torques_fl") # torques_fl = mul_vec_vec(fl_cnt_value, torques_fl, self.EntityName + "fused_torques_fl") torques_fr = selec_vector(self.wb_ctrl, 3, 6, self.EntityName + "torques_fr") # torques_fr = mul_vec_vec(fr_cnt_value, torques_fr, self.EntityName + "fused_torques_fr") torques_hl = selec_vector(self.wb_ctrl, 6, 9, self.EntityName + "torques_hl") # torques_hl = mul_vec_vec(hl_cnt_value, torques_hl, self.EntityName + "fused_torques_hl") torques_hr = selec_vector(self.wb_ctrl, 9, 12, self.EntityName + "torques_hr") # torques_hr = mul_vec_vec(hr_cnt_value, torques_hr, self.EntityName + "fused_torques_hr") torques_fl_6d = stack_two_vectors(torques_fl, zero_vec(3, "stack_fl_tau"), 3, 3) torques_fr_6d = stack_two_vectors(torques_fr, zero_vec(3, "stack_fr_tau"), 3, 3) torques_hl_6d = stack_two_vectors(torques_hl, zero_vec(3, "stack_hl_tau"), 3, 3) torques_hr_6d = stack_two_vectors(torques_hr, zero_vec(3, "stack_hr_tau"), 3, 3) torques_fl_fr = stack_two_vectors(torques_fl_6d, torques_fr_6d, 6, 6) torques_hl_hr = stack_two_vectors(torques_hl_6d, torques_hr_6d, 6, 6) wbc_torques = stack_two_vectors(torques_fl_fr, torques_hl_hr, 12, 12) ## hack to allow tracking of torques wbc_torques = mul_double_vec(1.0, wbc_torques, "com_torques") return wbc_torques def record_data(self): self.get_biased_base_position() self.get_biased_base_velocity() self.robot.add_trace(self.com_imp_ctrl.name, "des_pos") self.robot.add_trace(self.com_imp_ctrl.name, "des_vel") self.robot.add_trace(self.EntityName + '_biased_base_pos', 'sout') self.robot.add_trace(self.EntityName + '_biased_base_vel', 'sout') self.robot.add_trace(self.EntityName, "tau") # self.robot.add_ros_and_trace(self.EntityName, "tau") # # self.robot.add_trace(self.EntityName, "angtau") # self.robot.add_ros_and_trace(self.EntityName, "angtau") # # # self.robot.add_trace(self.EntityName, "wbctrl") # self.robot.add_ros_and_trace(self.EntityName, "wbctrl") # self.robot.add_trace(self.EntityName, "lqrtau") # self.robot.add_ros_and_trace(self.EntityName, "lqrtau") # self.robot.add_trace(self.EntityName, "thr_cnt_sensor") # self.robot.add_ros_and_trace(self.EntityName, "thr_cnt_sensor") # self.robot.add_trace("com_torques", "sout") # self.robot.add_ros_and_trace("com_torques", "sout") # self.robot.add_trace(self.EntityName, "end_eff_lqr_tau") # self.robot.add_ros_and_trace(self.EntityName, "end_eff_lqr_tau") # self.robot.add_trace("lqr_end_eff_force", "sout") # self.robot.add_ros_and_trace("lqr_end_eff_force", "sout") self.robot.add_trace("biased_pos", "sout") # self.robot.add_ros_and_trace("biased_pos", "sout") # self.robot.add_trace("biased_vel", "sout")