def main(robot): robot.timeStep = robot.device.getTimeStep() dt = robot.timeStep robot.traj_gen = create_config_trajectory_generator(dt, robot) JOINT = 31 QJOINT = JOINT + 6 # --- Joint robot.taskJoint = MetaTaskKineConfig(robot.dynamic, [QJOINT]) # robot.dynamic.com.recompute(0) robot.taskJoint.featureDes.errorIN.value = N_CONFIG * [0.0] robot.taskJoint.task.controlGain.value = 100 # --- CONTACTS # define contactLF and contactRF robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle']) robot.contactLF.feature.frame('desired') robot.contactLF.gain.setConstant(100) robot.contactLF.keep() locals()['contactLF'] = robot.contactLF robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) robot.contactRF.feature.frame('desired') robot.contactRF.gain.setConstant(100) robot.contactRF.keep() locals()['contactRF'] = robot.contactRF robot.sot = SOT('sot') robot.sot.setSize(robot.dynamic.getDimension()) plug(robot.sot.control, robot.device.control) robot.sot.push(robot.contactRF.task.name) robot.sot.push(robot.contactLF.task.name) robot.sot.push(robot.taskJoint.task.name) robot.device.control.recompute(0) plug(robot.traj_gen.x, robot.taskJoint.featureDes.errorIN) sleep(1.0) os.system('rosservice call /start_dynamic_graph') sleep(1.0) robot.traj_gen.move(QJOINT, -1.0, 1.0) sleep(1.1) robot.traj_gen.startSinusoid(QJOINT, 1.0, 8.0) sleep(10.0) robot.traj_gen.stop(QJOINT)
def createScrewTask(robot, TwoHandTool): refToTwoHandToolMatrix = array(RPYToMatrix(TwoHandTool)) #RH-TwoHandTool Homogeneous Transformation Matrix (fixed in time) robot.mTasks['rh'].feature.position.recompute(robot.device.control.time) RHToTwoHandToolMatrix = dot( linalg.inv(array(robot.mTasks['rh'].feature.position.value)), refToTwoHandToolMatrix) #!!!!!! RH and Support are different references, because the X rotation is not controlled in positioning!!!!!!!!!!!!!!!!!!!!!!!!!! RHToScrewMatrix = dot(RHToTwoHandToolMatrix, TwoHandToolToScrewMatrix) # Screw Lenght - unused at the moment #screw_len = 0.03 # TASK Screw robot.mTasks['screw'] = MetaTaskKine6d('screw', robot.dynamic, 'screw', 'right-wrist') handMgrip = array(robot.mTasks['rh'].opmodif) robot.mTasks['screw'].opmodif = matrixToTuple( dot(handMgrip, RHToScrewMatrix)) robot.mTasks['screw'].feature.selec.value = '000111' # TASK Screw orientation robot.mTasks['screw'].featureVec = FeatureVector3("featureVecScrew") plug(robot.mTasks['screw'].opPointModif.position, robot.mTasks['screw'].featureVec.signal('position')) plug(robot.mTasks['screw'].opPointModif.jacobian, robot.mTasks['screw'].featureVec.signal('Jq')) robot.mTasks['screw'].featureVec.vector.value = array([0., 0., 1.]) robot.mTasks['screw'].task.add(robot.mTasks['screw'].featureVec.name)
def __init__(self, footname, sotrobot, selec='111111'): robotname, sotjoint = parseHppName(footname) self.taskFoot = MetaTaskKine6d(Foot.sep + footname, sotrobot.dynamic, sotjoint, sotjoint) self.taskFoot.feature.selec.value = selec super(Foot, self).__init__( tasks=[ self.taskFoot.task, ], topics={ footname: { "velocity": False, "type": "matrixHomo", "handler": "hppjoint", "hppjoint": footname, "signalGetters": [self._signalPositionRef] }, # "vel_" + self.gripper.name: { "vel_" + footname: { "velocity": True, "type": "vector", "handler": "hppjoint", "hppjoint": footname, "signalGetters": [self._signalVelocityRef] }, }) self.taskFoot.gain.value = 5
def Pr2ContactTask(robot): task = MetaTaskKine6d('contact', robot.dynamic, 'contact', 'left-ankle') task.feature.frame('desired') task.feature.selec.value = '011100' task.gain.setConstant(10) #locals()['contact'] = task return task
def init_appli(robot): taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['right-wrist']) handMgrip = eye(4) handMgrip[0:3, 3] = (0.1, 0, 0) taskRH.opmodif = matrixToTuple(handMgrip) taskRH.feature.frame('desired') # --- STATIC COM (if not walking) taskCom = MetaTaskKineCom(robot.dynamic) robot.dynamic.com.recompute(0) taskCom.featureDes.errorIN.value = robot.dynamic.com.value taskCom.task.controlGain.value = 10 # --- CONTACTS contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle']) contactLF.feature.frame('desired') contactLF.gain.setConstant(10) contactLF.keep() locals()['contactLF'] = contactLF contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) contactRF.feature.frame('desired') contactRF.gain.setConstant(10) contactRF.keep() locals()['contactRF'] = contactRF from dynamic_graph import plug from dynamic_graph.sot.core.sot import SOT sot = SOT('sot') sot.setSize(robot.dynamic.getDimension()) plug(sot.control, robot.device.control) from dynamic_graph.ros import RosPublish ros_publish_state = RosPublish("ros_publish_state") ros_publish_state.add("vector", "state", "/sot_control/state") from dynamic_graph import plug plug(robot.device.state, ros_publish_state.state) robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100) sot.push(contactRF.task.name) sot.push(contactLF.task.name) sot.push(taskCom.task.name) robot.device.control.recompute(0)
def Pr2BaseTask(robot): task = MetaTaskKine6d('base', robot.dynamic, 'waist', 'waist') baseMground = eye(4) baseMground[0:3, 3] = (0, 0, 0) task.opmodif = matrixToTuple(baseMground) task.feature.frame('desired') task.feature.selec.value = '100011' task.gain.setConstant(10) return task
def Pr2ContactTask(robot): task = MetaTaskKine6d('contact', robot.dynamic, 'contact', 'left-ankle') task.feature.frame('desired') task.feature.selec.value = '011100' task.featureDes.position.value = \ array([[1,0,0,0],[0,1,0,0],[0,0,1,0.051],[0,0,0,1]]) task.gain.setConstant(10) #locals()['contact'] = task return task
def main(robot, conf=None): if (conf is None): conf = get_default_conf() robot.timeStep = robot.device.getTimeStep() dt = robot.timeStep NJ = robot.dimension - 7 robot.comTrajGen = create_com_trajectory_generator(dt, robot) # --- COM robot.taskCom = MetaTaskKineCom(robot.dynamic) robot.dynamic.com.recompute(0) robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value robot.taskCom.task.controlGain.value = 10 # --- CONTACTS # define contactLF and contactRF robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle']) robot.contactLF.feature.frame('desired') robot.contactLF.gain.setConstant(100) robot.contactLF.keep() locals()['contactLF'] = robot.contactLF robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) robot.contactRF.feature.frame('desired') robot.contactRF.gain.setConstant(100) robot.contactRF.keep() locals()['contactRF'] = robot.contactRF # --- SOT robot.sot = SOT('sot') robot.sot.setSize(robot.dynamic.getDimension()) plug(robot.sot.control, robot.device.control) robot.sot.push(robot.contactRF.task.name) robot.sot.push(robot.taskCom.task.name) robot.sot.push(robot.contactLF.task.name) robot.device.control.recompute(0) # --- ENTITIES robot.param_server = create_parameter_server(conf.param_server, dt) robot.example = create_example()
def createTasks(robot): # MetaTasks dictonary robot.mTasks = dict() robot.tasksIne = dict() # Foot contacts robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle']) robot.contactLF.feature.frame('desired') robot.contactLF.gain.setConstant(10) robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) robot.contactRF.feature.frame('desired') robot.contactRF.gain.setConstant(10) # MetaTasksKine6d for other operational points robot.mTasks['waist'] = MetaTaskKine6d('waist', robot.dynamic, 'waist', robot.OperationalPointsMap['waist']) robot.mTasks['chest'] = MetaTaskKine6d('chest', robot.dynamic, 'chest', robot.OperationalPointsMap['chest']) robot.mTasks['rh'] = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['right-wrist']) robot.mTasks['lh'] = MetaTaskKine6d('lh', robot.dynamic, 'lh', robot.OperationalPointsMap['left-wrist']) for taskName in robot.mTasks: robot.mTasks[taskName].feature.frame('desired') robot.mTasks[taskName].gain.setConstant(10) handMgrip = eye(4) handMgrip[0:3, 3] = (0, 0, -0.14) robot.mTasks['rh'].opmodif = matrixToTuple(handMgrip) robot.mTasks['lh'].opmodif = matrixToTuple(handMgrip) # CoM Task robot.mTasks['com'] = MetaTaskKineCom(robot.dynamic) robot.dynamic.com.recompute(0) robot.mTasks['com'].featureDes.errorIN.value = robot.dynamic.com.value robot.mTasks['com'].task.controlGain.value = 10 robot.mTasks['com'].feature.selec.value = '011' # Posture Task robot.mTasks['posture'] = MetaTaskKinePosture(robot.dynamic) robot.mTasks['posture'].ref = robot.halfSitting robot.mTasks['posture'].gain.setConstant(5) # TASK INEQUALITY # Task Height featureHeight = FeatureGeneric('featureHeight') plug(robot.dynamic.com, featureHeight.errorIN) plug(robot.dynamic.Jcom, featureHeight.jacobianIN) robot.tasksIne['taskHeight'] = TaskInequality('taskHeight') robot.tasksIne['taskHeight'].add(featureHeight.name) robot.tasksIne['taskHeight'].selec.value = '100' robot.tasksIne['taskHeight'].referenceInf.value = (0., 0., 0.) # Xmin, Ymin robot.tasksIne['taskHeight'].referenceSup.value = (0., 0., 0.80771) # Xmax, Ymax robot.tasksIne['taskHeight'].dt.value = robot.timeStep
def makeTasks(self, sotrobot): from dynamic_graph.sot.core.matrix_util import matrixToTuple from dynamic_graph.sot.core import Multiply_of_matrix, Multiply_of_matrixHomo, OpPointModifier from dynamic_graph import plug if self.relative: self.graspTask = MetaTaskKine6d ( Grasp.sep + self.gripper.name + Grasp.sep + self.otherGripper.name, sotrobot.dynamic, self.gripper.sotjoint, self.gripper.sotjoint) # Creates the matrix transforming the goal gripper's position into the desired moving gripper's position: # on the other handle of the object #M = ((-1.0, 0.0, 0.0, 0.0), (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, -1.0, -0.55), (0.0, 0.0, 0.0, 1.0)) M = transformToMatrix(self.otherGripper.sotpose * self.otherHandle.sotpose.inverse() * self.handle.sotpose * self.gripper.sotpose.inverse()) self.graspTask.opmodif = matrixToTuple(M) self.graspTask.feature.frame("current") setGain(self.graspTask.gain,(100,0.9,0.01,0.9)) self.graspTask.task.setWithDerivative (False) # Sets the position and velocity of the other gripper as the goal of the first gripper pose if not self.opPointExist(sotrobot.dynamic, self.otherGripper.sotjoint): sotrobot.dynamic.createOpPoint(self.otherGripper.sotjoint, self.otherGripper.sotjoint) sotrobot.dynamic.createVelocity("vel_"+self.otherGripper.sotjoint, self.otherGripper.sotjoint) plug(sotrobot.dynamic.signal(self.otherGripper.sotjoint), self.graspTask.featureDes.position) plug(sotrobot.dynamic.signal("vel_"+self.otherGripper.sotjoint), self.graspTask.featureDes.velocity) #plug(sotrobot.dynamic.signal("J"+self.otherGripper.sotjoint), self.graspTask.featureDes.Jq) # We will need a less barbaric method to do this... # Zeroes the jacobian for the joints we don't want to use. # - Create the selection matrix for the desired joints mat = () for i in range(38): mat += ((0,)*i + (1 if i in range(29, 36) else 0,) + (0,)*(37-i),) # - Multiplies it with the computed jacobian matrix of the gripper mult = Multiply_of_matrix('mult') mult.sin2.value = mat plug(self.graspTask.opPointModif.jacobian, mult.sin1) plug(mult.sout, self.graspTask.feature.Jq) self.tasks = [ self.graspTask.task ] #self.tasks = [] self += EEPosture (sotrobot, self.gripper, [-0.2 if self.closeGripper else 0])
def main(robot): robot.timeStep = robot.device.getTimeStep() dt = robot.timeStep # --- COM robot.taskCom = MetaTaskKineCom(robot.dynamic) robot.dynamic.com.recompute(0) robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value robot.taskCom.task.controlGain.value = 10 robot.estimator = create_dummy_dcm_estimator(robot) # --- Admittance controller Kp = [0.0, 0.0, 0.0] robot.com_admittance_control = create_com_admittance_controller( Kp, dt, robot) # --- CONTACTS # define contactLF and contactRF robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle']) robot.contactLF.feature.frame('desired') robot.contactLF.gain.setConstant(100) robot.contactLF.keep() locals()['contactLF'] = robot.contactLF robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) robot.contactRF.feature.frame('desired') robot.contactRF.gain.setConstant(100) robot.contactRF.keep() locals()['contactRF'] = robot.contactRF robot.sot = SOT('sot') robot.sot.setSize(robot.dynamic.getDimension()) plug(robot.sot.control, robot.device.control) robot.sot.push(robot.contactRF.task.name) robot.sot.push(robot.contactLF.task.name) robot.sot.push(robot.taskCom.task.name) robot.device.control.recompute(0) # --- TRACER robot.tracer = TracerRealTime("zmp_tracer") robot.tracer.setBufferSize(80 * (2**20)) robot.tracer.open('/tmp', 'dg_', '.dat') robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name)) addTrace(robot.tracer, robot.dynamic, 'zmp') addTrace(robot.tracer, robot.estimator, 'dcm') # SIMULATION plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN) sleep(1.0) os.system("rosservice call \start_dynamic_graph") sleep(2.0) plug(robot.estimator.dcm, robot.com_admittance_control.zmpDes) robot.com_admittance_control.setState(robot.dynamic.com.value, [0.0, 0.0, 0.0]) robot.com_admittance_control.Kp.value = [10.0, 10.0, 0.0] robot.tracer.start() sleep(5.0) dump_tracer(robot.tracer) # --- DISPLAY zmp_data = np.loadtxt('/tmp/dg_' + robot.dynamic.name + '-zmp.dat') zmpDes_data = np.loadtxt('/tmp/dg_' + robot.estimator.name + '-dcm.dat') plt.figure() plt.plot(zmp_data[:, 1], 'b-') plt.plot(zmpDes_data[:, 1], 'b--') plt.plot(zmp_data[:, 2], 'r-') plt.plot(zmpDes_data[:, 2], 'r--') plt.title('ZMP real vs desired') plt.legend(['Real x', 'Desired x', 'Real y', 'Desired y']) plt.figure() plt.plot(zmp_data[:, 1] - zmpDes_data[:, 1], 'b-') plt.plot(zmp_data[:, 2] - zmpDes_data[:, 2], 'r-') plt.title('ZMP error') plt.legend(['Error on x', 'Error on y']) plt.show()
from dynamic_graph import plug from dynamic_graph.sot.core import SOT sot = SOT('sot') sot.setSize(robot.dynamic.getDimension()) plug(sot.control, robot.device.control) # ------------------------------------------------------------------------------ # ---- TASKS ------------------------------------------------------------------- # ---- TASK GRIP from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd from dynamic_graph.sot.core.matrix_util import matrixToTuple from numpy import eye taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['right-wrist']) handMgrip = eye(4) handMgrip[0:3, 3] = (0.1, 0, 0) taskRH.opmodif = matrixToTuple(handMgrip) taskRH.feature.frame('desired') # --- STATIC COM (if not walking) taskCom = MetaTaskKineCom(robot.dynamic) robot.dynamic.com.recompute(0) taskCom.featureDes.errorIN.value = robot.dynamic.com.value taskCom.task.controlGain.value = 10 # --- CONTACTS #define contactLF and contactRF for name, joint in [['LF', robot.OperationalPointsMap['left-ankle']], ['RF', robot.OperationalPointsMap['right-ankle']]]: contact = MetaTaskKine6d('contact' + name, robot.dynamic, name, joint)
robot.e2q = EulerToQuat("e2q") plug(robot.baseEstimator.q, robot.e2q.euler) robot.forceCalibrator = create_ft_wrist_calibrator(robot, endEffectorWeight, rightOC, leftOC) robot.controller = create_end_effector_admittance_controller( robot, endEffector, "EEAdmittance") robot.controlManager = create_ctrl_manager(controlManagerConfig, robot.timeStep) robot.controlManager.addCtrlMode('sot_input') robot.controlManager.setCtrlMode('all', 'sot_input') # --- HAND TASK ---------------------------------------------------------------- taskRightHand = MetaTaskKine6d('rh', robot.dynamic, 'rh', 'arm_right_7_joint') handMgrip = np.eye(4) handMgrip[0:3, 3] = (0.1, 0, 0) taskRightHand.opmodif = matrixToTuple(handMgrip) taskRightHand.feature.frame('desired') taskRightHand.feature.selec.value = '111111' taskRightHand.task.setWithDerivative(True) taskRightHand.task.controlGain.value = 0 taskRightHand.feature.position.value = np.eye(4) taskRightHand.feature.velocity.value = [0., 0., 0., 0., 0., 0.] taskRightHand.featureDes.position.value = np.eye(4) plug(robot.controller.dq, taskRightHand.featureDes.velocity) # --- BASE TASK ---------------------------------------------------------------- taskWaist = MetaTaskKine6d('taskWaist', robot.dynamic, 'WT',
def Pr2LeftHandTask(robot): task = MetaTaskKine6d('lh', robot.dynamic, 'lh', 'left-wrist') task.opmodif = matrixToTuple(Pr2handMgrip) task.feature.frame('desired') return task
# --- COM height robot.taskComH = MetaTaskKineCom(robot.dynamic, name='comH') plug(robot.wp.comDes, robot.taskComH.featureDes.errorIN) robot.taskComH.task.controlGain.value = 100. robot.taskComH.feature.selec.value = '100' # --- COM robot.taskCom = MetaTaskKineCom(robot.dynamic) plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN) plug(robot.com_admittance_control.dcomRef, robot.taskCom.featureDes.errordotIN) robot.taskCom.task.controlGain.value = 100. robot.taskCom.task.setWithDerivative(True) robot.taskCom.feature.selec.value = '011' # --- Waist robot.keepWaist = MetaTaskKine6d('keepWaist', robot.dynamic, 'WT', robot.OperationalPointsMap['waist']) robot.keepWaist.feature.frame('desired') robot.keepWaist.gain.setConstant(300) plug(robot.wp.waistDes, robot.keepWaist.featureDes.position) robot.keepWaist.feature.selec.value = '111000' locals()['keepWaist'] = robot.keepWaist # --- SOT solver robot.sot = SOT('sot') robot.sot.setSize(robot.dynamic.getDimension()) # --- Plug SOT control to device through control manager plug(robot.sot.control, robot.cm.ctrl_sot_input) plug(robot.cm.u_safe, robot.device.control) robot.sot.push(robot.taskUpperBody.name)
def _makeAbsolute(self, sotrobot, withMeasurementOfObjectPos, withMeasurementOfGripperPos): name = PreGrasp.sep.join( ["", "pregrasp", self.gripper.name, self.handle.fullName]) self.graspTask = MetaTaskKine6d(name, sotrobot.dynamic, self.gripper.joint, self.gripper.joint) setGain(self.graspTask.gain, (4.9, 0.9, 0.01, 0.9)) self.graspTask.task.setWithDerivative(False) # Current gripper position self.graspTask.opmodif = se3ToTuple(self.gripper.pose) # Express the velocities in local frame. This is the default. # self.graspTask.opPointModif.setEndEffector(True) # Desired gripper position: # Planned handle pose H_p = object_planned_pose * self.handle.pose # Real handle pose H_r = object_real_pose * self.handle.pose # Displacement M = H_p^-1 * H_r # planned gripper pose G_p= joint_planned_pose * self.gripper.pose # The derised position is # G*_r = G_p * M = G_p * h^-1 * O_p^-1 * O_r * h # = J_p * g * h^-1 * O_p^-1 * O_r * h self.gripper_desired_pose = Multiply_of_matrixHomo(name + "_desired") nsignals = 2 if withMeasurementOfGripperPos: nsignals += 1 if withMeasurementOfObjectPos: nsignals += 5 self.gripper_desired_pose.setSignalNumber(nsignals) isignal = 0 def sig(i): return self.gripper_desired_pose.signal("sin" + str(i)) # Object calibration if withMeasurementOfObjectPos: h = self.handle.pose # TODO Integrate measurement of h_r: position error of O_r^-1 * G_r # (for the release phase and computed only at time 0) # self.gripper_desired_pose.sin0 -> plug to handle link planning pose self.topics[self.handle.fullLink] = { "velocity": False, "type": "matrixHomo", "handler": "hppjoint", "hppjoint": self.handle.fullLink, "signalGetters": [lambda: self.gripper_desired_pose.sin0], } isignal += 1 sig(isignal).value = se3ToTuple(h.inverse()) isignal += 1 delta_h = sig(isignal) self.topics["delta_" + self.handle.fullLink] = { "velocity": False, "type": "matrixHomo", "handler": "tf_listener", "frame0": self.handle.fullLink, "frame1": self.handle.fullLink + "_estimated", "signalGetters": [lambda: delta_h], } isignal += 1 sig(isignal).value = se3ToTuple(h) isignal += 1 self._oMh_p_inv = Inverse_of_matrixHomo(self.handle.fullLink + "_invert_planning_pose") self.topics[self.handle.fullLink]["signalGetters"] \ .append (lambda: self._oMh_p_inv.sin) plug(self._oMh_p_inv.sout, sig(isignal)) isignal += 1 # Joint planning pose joint_planning_pose = sig(isignal) self.topics[self.gripper.fullJoint] = { "velocity": False, "type": "matrixHomo", "handler": "hppjoint", "hppjoint": self.gripper.fullJoint, "signalGetters": [lambda: joint_planning_pose], } isignal += 1 # Joint calibration if withMeasurementOfGripperPos: self._delta_g_inv = Inverse_of_matrixHomo(self.gripper.fullJoint + "_delta_inv") self.topics["delta_" + self.gripper.fullJoint] = { "velocity": False, "type": "matrixHomo", "handler": "tf_listener", "frame0": self.gripper.fullJoint, "frame1": self.gripper.fullJoint + "_estimated", "signalGetters": [lambda: self._delta_g_inv.sin], } plug(self._delta_g_inv.sout, self.gripper_desired_pose.sin6) isignal += 1 sig(isignal).value = se3ToTuple(self.gripper.pose) isignal += 1 assert isignal == nsignals plug(self.gripper_desired_pose.sout, self.graspTask.featureDes.position) self.tasks = [self.graspTask.task]
display=True) pinocchioRobot.initDisplay(loadModel=True) pinocchioRobot.display(fromSotToPinocchio(initialConfig)) robot = HumanoidRobot(robotName, pinocchioRobot.model, pinocchioRobot.data, initialConfig, OperationalPointsMap) njoints = pinocchioRobot.model.nv - 6 sot = SOT('sot') sot.setSize(robot.dynamic.getDimension()) plug(sot.control, robot.device.control) # ------------------------------------------------------------------------------ # ---- TASKS ------------------------------------------------------------------- taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['right-wrist']) handMgrip = eye(4) handMgrip[0:3, 3] = (0.1, 0, 0) taskRH.opmodif = matrixToTuple(handMgrip) taskRH.feature.frame('desired') taskLH = MetaTaskKine6d('lh', robot.dynamic, 'lh', robot.OperationalPointsMap['left-wrist']) taskLH.opmodif = matrixToTuple(handMgrip) taskLH.feature.frame('desired') # --- STATIC COM (if not walking) taskCom = MetaTaskKineCom(robot.dynamic) robot.dynamic.com.recompute(0) taskCom.featureDes.errorIN.value = robot.dynamic.com.value taskCom.task.controlGain.value = 10
def Pr2ChestTask(robot): task = MetaTaskKine6d('chest', robot.dynamic, 'chest', 'chest') task.feature.frame('desired') return task
def _makeAbsoluteBasedOnOther(self, sotrobot, withMeasurementOfObjectPos, withMeasurementOfGripperPos): assert self.handle.robotName == self.otherHandle.robotName assert self.handle.link == self.otherHandle.link name = PreGrasp.sep.join([ "", "pregrasp", self.gripper.fullName, self.handle.fullName, "based", self.otherGripper.name, self.otherHandle.fullName ]) self.graspTask = MetaTaskKine6d(name, sotrobot.dynamic, self.otherGripper.joint, self.otherGripper.joint) setGain(self.graspTask.gain, (4.9, 0.9, 0.01, 0.9)) self.graspTask.task.setWithDerivative(False) # Current gripper position self.graspTask.opmodif = se3ToTuple(self.otherGripper.pose) # Express the velocities in local frame. This is the default. # self.graspTask.opPointModif.setEndEffector(True) # Desired gripper position: # Displacement gripper1: M = G1_p^-1 * G1_r # The derised position of handle1 is # H1*_r = H1_p * M = H1_p * G1_p^-1 * G1_r # # Moreover, the relative position of handle2 wrt to gripper2 may not be perfect so # h2_r = O_r^-1 * G2_r # # G2*_r = O*_r * h2_r = H1_p * G1_p^-1 * G1_r * h1^-1 * h2_r # O_p * h1 * G1_p^-1 * G1_r * h1^-1 * h2_r # O_p * h1 * g1^-1 * J1_p^-1 * J1_r * g1 * h1^-1 * h2_r # where h2_r can be a constant value of the expression above. self.gripper_desired_pose = Multiply_of_matrixHomo(name + "_desired") if withMeasurementOfObjectPos: # TODO Integrate measurement of h1_r and h2_r: position error # O_r^-1 * G1_r and O_r^-1 * G2_r # (for the release phase and computed only at time 0) self.gripper_desired_pose.setSignalNumber(5) ## self.gripper_desired_pose.setSignalNumber (7) # self.gripper_desired_pose.sin0 -> plug to object1 planning pose # self.gripper_desired_pose.sin1.value = se3ToTuple(self.handle.pose) self.gripper_desired_pose.sin1.value = se3ToTuple( self.handle.pose * self.gripper.pose.inverse()) self._invert_planning_pose = Inverse_of_matrixHomo( self.gripper.fullLink + "_invert_planning_pose") # self._invert_planning_pose.sin -> plug to link1 planning pose plug(self._invert_planning_pose.sout, self.gripper_desired_pose.sin2) # self.gripper_desired_pose.sin3 -> plug to link1 real pose # self.gripper_desired_pose.sin4.value = se3ToTuple (self.handle.pose.inverse() * self.otherHandle.pose) self.gripper_desired_pose.sin4.value = se3ToTuple( self.gripper.pose * self.handle.pose.inverse() * self.otherHandle.pose) ## self.gripper_desired_pose.sin4.value = se3ToTuple (self.gripper.pose * self.handle.pose.inverse()) ## self.gripper_desired_pose.sin5 -> plug to object real pose ## if self.otherGripper.joint not in sotrobot.dyn.signals(): ## sotrobot.dyn.createOpPoint (self.otherGripper.joint, self.otherGripper.joint) ## plug(sotrobot.dyn.signal(self.otherGripper.joint), self.gripper_desired_pose.sin6) plug(self.gripper_desired_pose.sout, self.graspTask.featureDes.position) self.topics = { self.handle.fullLink: { "velocity": False, "type": "matrixHomo", "handler": "hppjoint", "hppjoint": self.handle.fullLink, "signalGetters": [ lambda: self.gripper_desired_pose.sin0, ] }, self.gripper.fullLink: { "velocity": False, "type": "matrixHomo", "handler": "hppjoint", "hppjoint": self.gripper.fullLink, "signalGetters": [ lambda: self._invert_planning_pose.sin, ] }, "measurement_" + self.gripper.fullLink: { "velocity": False, "type": "matrixHomo", "handler": "tf_listener", "frame0": "world", "frame1": self.gripper.fullLink, "signalGetters": [ lambda: self.gripper_desired_pose.sin3, ] }, ## "measurement_" + self.otherHandle.fullLink: { ## "velocity": False, ## "type": "matrixHomo", ## "handler": "tf_listener", ## "frame0": "world", ## "frame1": self.otherHandle.fullLink, ## "signalGetters": [ self.gripper_desired_pose.sin5 ] }, } else: # G2*_r = H1_p * h1^-1 * h2 = G2_p = J2_p * G self.gripper_desired_pose.setSignalNumber(2) # self.gripper_desired_pose.sin0 -> plug to joint planning pose self.gripper_desired_pose.sin1.value = se3ToTuple( self.otherGripper.pose) self.topics = { self.otherGripper.fullJoint: { "velocity": False, "type": "matrixHomo", "handler": "hppjoint", "hppjoint": self.otherGripper.fullJoint, "signalGetters": [ lambda: self.gripper_desired_pose.sin0, ] }, } plug(self.gripper_desired_pose.sout, self.graspTask.featureDes.position) self.tasks = [self.graspTask.task]
import sot_talos_balance.talos.parameter_server_conf as param_server_conf from sot_talos_balance.create_entities_utils import * robot.timeStep = robot.device.getTimeStep() dt = robot.timeStep robot.comTrajGen = create_com_trajectory_generator(dt, robot) # --- COM robot.taskCom = MetaTaskKineCom(robot.dynamic) robot.dynamic.com.recompute(0) robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value robot.taskCom.task.controlGain.value = 10 # --- CONTACTS #define contactLF and contactRF robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle']) robot.contactLF.feature.frame('desired') robot.contactLF.gain.setConstant(0) robot.contactLF.keep() robot.contactLF.task.setWithDerivative(True) robot.contactLF.featureDes.velocity.value = [0.] * 6 locals()['contactLF'] = robot.contactLF robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) robot.contactRF.feature.frame('desired') robot.contactRF.gain.setConstant(0) robot.contactRF.keep() robot.contactRF.task.setWithDerivative(True) robot.contactRF.featureDes.velocity.value = [0.] * 6 locals()['contactRF'] = robot.contactRF
def Pr2LeftHandTask(robot): task = MetaTaskKine6d('left-wrist', robot.dynamic, 'left-wrist', 'left-wrist') task.feature.frame('desired') return task
# 2. Ros binding # roscore must be running from dynamic_graph.ros import Ros ros = Ros(robot) # 3. Create a solver from dynamic_graph.sot.application.velocity.precomputed_tasks import Solver solver = Solver(robot) # 4. Define a position task for the right hand from dynamic_graph.sot.core.meta_tasks_kine import gotoNd, MetaTaskKine6d from numpy import eye from dynamic_graph.sot.core.matrix_util import matrixToTuple taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', 'arm_joint_5') Pr2handMgrip = eye(4) Pr2handMgrip[0:3, 3] = (0.337, 0, 0.275) taskRH.opmodif = matrixToTuple(Pr2handMgrip) taskRH.feature.frame('desired') targetR = (0.65, 0.2, 0.9) selec = '111' gain = (4.9, 0.9, 0.01, 0.9) gotoNd(taskRH, targetR, selec, gain) # 5. Add a contact constraint with the robot and the floor contact = MetaTaskKine6d('contact', robot.dynamic, 'contact', 'left-ankle') contact.feature.frame('desired') contact.feature.selec.value = '011100' contact.gain.setConstant(10) contact.keep()
robot = Pr2('PR2', device=RobotSimu('PR2')) plug(robot.device.state, robot.dynamic.position) # 2. Ros binding # roscore must be running from dynamic_graph.ros import Ros ros = Ros(robot) # Use kine solver (with inequalities) solver = initialize(robot) # 4. Define a position task for the right hand from dynamic_graph.sot.core.meta_tasks_kine import gotoNd, MetaTaskKine6d from numpy import eye from dynamic_graph.sot.core.matrix_util import matrixToTuple taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', 'right-wrist') Pr2handMgrip = eye(4) Pr2handMgrip[0:3, 3] = (0.18, 0, 0) taskRH.opmodif = matrixToTuple(Pr2handMgrip) taskRH.feature.frame('desired') targetR = (0.65, 0.2, 0.9) selec = '111' gain = (4.9, 0.9, 0.01, 0.9) gotoNd(taskRH, targetR, selec, gain) # 6. Push tasks in the solver solver.push(taskRH.task) # Main loop dt = 3e-3 from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts
# flake8: noqa from dynamic_graph import plug from dynamic_graph.ros import RosPublish from dynamic_graph.sot.core import SOT from dynamic_graph.sot.core.matrix_util import matrixToTuple from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd from dynamic_graph.sot.tiago.diff_drive_controller import HolonomicProjection from numpy import eye taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['wrist']) handMgrip = eye(4) handMgrip[0:3, 3] = (0.1, 0, 0) taskRH.opmodif = matrixToTuple(handMgrip) taskRH.feature.frame('desired') projection = HolonomicProjection("projection") projection.setSize(robot.dynamic.getDimension()) projection.setLeftWheel(6) projection.setRightWheel(7) # The wheel separation could be obtained with pinocchio. # See pmb2_description/urdf/base.urdf.xacro projection.setWheelRadius(0.0985) projection.setWheelSeparation(0.4044) plug(robot.dynamic.mobilebase, projection.basePose) sot = SOT('sot') sot.setSize(robot.dynamic.getDimension()) plug(projection.projection, sot.proj0)
def init_sot_talos_balance(robot, test_folder): cm_conf.CTRL_MAX = 1000.0 # temporary hack dt = robot.device.getTimeStep() robot.timeStep = dt # --- Pendulum parameters robot_name = 'robot' robot.dynamic.com.recompute(0) robotDim = robot.dynamic.getDimension() mass = robot.dynamic.data.mass[0] h = robot.dynamic.com.value[2] g = 9.81 omega = sqrt(g / h) # --- Parameter server robot.param_server = create_parameter_server(param_server_conf, dt) # --- Initial feet and waist robot.dynamic.createOpPoint('LF', robot.OperationalPointsMap['left-ankle']) robot.dynamic.createOpPoint('RF', robot.OperationalPointsMap['right-ankle']) robot.dynamic.createOpPoint('WT', robot.OperationalPointsMap['waist']) robot.dynamic.LF.recompute(0) robot.dynamic.RF.recompute(0) robot.dynamic.WT.recompute(0) # -------------------------- DESIRED TRAJECTORY -------------------------- rospack = RosPack() data_folder = rospack.get_path('sot-talos-balance') + "/data/" folder = data_folder + test_folder + '/' # --- Trajectory generators # --- General trigger robot.triggerTrajGen = BooleanIdentity('triggerTrajGen') robot.triggerTrajGen.sin.value = 0 # --- CoM robot.comTrajGen = create_com_trajectory_generator(dt, robot) robot.comTrajGen.x.recompute(0) # trigger computation of initial value robot.comTrajGen.playTrajectoryFile(folder + 'CoM.dat') plug(robot.triggerTrajGen.sout, robot.comTrajGen.trigger) # --- Left foot robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'LF') robot.lfTrajGen.x.recompute(0) # trigger computation of initial value robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m') plug(robot.lfTrajGen.x, robot.lfToMatrix.sin) robot.lfTrajGen.playTrajectoryFile(folder + 'LeftFoot.dat') plug(robot.triggerTrajGen.sout, robot.lfTrajGen.trigger) # --- Right foot robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'RF') robot.rfTrajGen.x.recompute(0) # trigger computation of initial value robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m') plug(robot.rfTrajGen.x, robot.rfToMatrix.sin) robot.rfTrajGen.playTrajectoryFile(folder + 'RightFoot.dat') plug(robot.triggerTrajGen.sout, robot.rfTrajGen.trigger) # --- ZMP robot.zmpTrajGen = create_zmp_trajectory_generator(dt, robot) robot.zmpTrajGen.x.recompute(0) # trigger computation of initial value # robot.zmpTrajGen.playTrajectoryFile(folder + 'ZMP.dat') plug(robot.triggerTrajGen.sout, robot.zmpTrajGen.trigger) # --- Waist robot.waistTrajGen = create_orientation_rpy_trajectory_generator( dt, robot, 'WT') robot.waistTrajGen.x.recompute(0) # trigger computation of initial value robot.waistMix = Mix_of_vector("waistMix") robot.waistMix.setSignalNumber(3) robot.waistMix.addSelec(1, 0, 3) robot.waistMix.addSelec(2, 3, 3) robot.waistMix.default.value = [0.0] * 6 robot.waistMix.signal("sin1").value = [0.0] * 3 plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2")) robot.waistToMatrix = PoseRollPitchYawToMatrixHomo('w2m') plug(robot.waistMix.sout, robot.waistToMatrix.sin) robot.waistTrajGen.playTrajectoryFile(folder + 'WaistOrientation.dat') plug(robot.triggerTrajGen.sout, robot.waistTrajGen.trigger) # --- Interface with controller entities wp = DummyWalkingPatternGenerator('dummy_wp') wp.init() wp.omega.value = omega #wp.waist.value = robot.dynamic.WT.value # wait receives a full homogeneous matrix, but only the rotational part is controlled #wp.footLeft.value = robot.dynamic.LF.value #wp.footRight.value = robot.dynamic.RF.value #wp.com.value = robot.dynamic.com.value #wp.vcom.value = [0.]*3 #wp.acom.value = [0.]*3 plug(robot.waistToMatrix.sout, wp.waist) plug(robot.lfToMatrix.sout, wp.footLeft) plug(robot.rfToMatrix.sout, wp.footRight) plug(robot.comTrajGen.x, wp.com) plug(robot.comTrajGen.dx, wp.vcom) plug(robot.comTrajGen.ddx, wp.acom) #plug(robot.zmpTrajGen.x, wp.zmp) robot.wp = wp # --- Compute the values to use them in initialization robot.wp.comDes.recompute(0) robot.wp.dcmDes.recompute(0) robot.wp.zmpDes.recompute(0) # -------------------------- ESTIMATION -------------------------- # --- Base Estimation robot.device_filters = create_device_filters(robot, dt) robot.imu_filters = create_imu_filters(robot, dt) robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf) robot.m2qLF = MatrixHomoToPoseQuaternion('m2qLF') plug(robot.dynamic.LF, robot.m2qLF.sin) plug(robot.m2qLF.sout, robot.base_estimator.lf_ref_xyzquat) robot.m2qRF = MatrixHomoToPoseQuaternion('m2qRF') plug(robot.dynamic.RF, robot.m2qRF.sin) plug(robot.m2qRF.sout, robot.base_estimator.rf_ref_xyzquat) # --- Conversion e2q = EulerToQuat('e2q') plug(robot.base_estimator.q, e2q.euler) robot.e2q = e2q # --- Kinematic computations robot.rdynamic = DynamicPinocchio("real_dynamics") robot.rdynamic.setModel(robot.dynamic.model) robot.rdynamic.setData(robot.rdynamic.model.createData()) plug(robot.base_estimator.q, robot.rdynamic.position) robot.rdynamic.velocity.value = [0.0] * robotDim robot.rdynamic.acceleration.value = [0.0] * robotDim # --- CoM Estimation cdc_estimator = DcmEstimator('cdc_estimator') cdc_estimator.init(dt, robot_name) plug(robot.e2q.quaternion, cdc_estimator.q) plug(robot.base_estimator.v, cdc_estimator.v) robot.cdc_estimator = cdc_estimator # --- DCM Estimation estimator = DummyDcmEstimator("dummy") estimator.omega.value = omega estimator.mass.value = 1.0 plug(robot.cdc_estimator.c, estimator.com) plug(robot.cdc_estimator.dc, estimator.momenta) estimator.init() robot.estimator = estimator # --- Force calibration robot.ftc = create_ft_calibrator(robot, ft_conf) # --- ZMP estimation zmp_estimator = SimpleZmpEstimator("zmpEst") robot.rdynamic.createOpPoint('sole_LF', 'left_sole_link') robot.rdynamic.createOpPoint('sole_RF', 'right_sole_link') plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft) plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight) plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft) plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight) zmp_estimator.init() robot.zmp_estimator = zmp_estimator # -------------------------- ADMITTANCE CONTROL -------------------------- # --- DCM controller Kp_dcm = [8.0] * 3 Ki_dcm = [0.0, 0.0, 0.0] # zero (to be set later) gamma_dcm = 0.2 dcm_controller = DcmController("dcmCtrl") dcm_controller.Kp.value = Kp_dcm dcm_controller.Ki.value = Ki_dcm dcm_controller.decayFactor.value = gamma_dcm dcm_controller.mass.value = mass dcm_controller.omega.value = omega plug(robot.cdc_estimator.c, dcm_controller.com) plug(robot.estimator.dcm, dcm_controller.dcm) plug(robot.wp.zmpDes, dcm_controller.zmpDes) plug(robot.wp.dcmDes, dcm_controller.dcmDes) dcm_controller.init(dt) robot.dcm_control = dcm_controller Ki_dcm = [1.0, 1.0, 1.0] # this value is employed later # --- CoM admittance controller Kp_adm = [0.0, 0.0, 0.0] # zero (to be set later) com_admittance_control = ComAdmittanceController("comAdmCtrl") com_admittance_control.Kp.value = Kp_adm plug(robot.zmp_estimator.zmp, com_admittance_control.zmp) com_admittance_control.zmpDes.value = robot.wp.zmpDes.value # should be plugged to robot.dcm_control.zmpRef plug(robot.wp.acomDes, com_admittance_control.ddcomDes) com_admittance_control.init(dt) com_admittance_control.setState(robot.wp.comDes.value, [0.0, 0.0, 0.0]) robot.com_admittance_control = com_admittance_control # --- Control Manager robot.cm = create_ctrl_manager(cm_conf, dt, robot_name='robot') robot.cm.addCtrlMode('sot_input') robot.cm.setCtrlMode('all', 'sot_input') robot.cm.addEmergencyStopSIN('zmp') # -------------------------- SOT CONTROL -------------------------- # --- Upper body robot.taskUpperBody = Task('task_upper_body') robot.taskUpperBody.feature = FeaturePosture('feature_upper_body') q = list(robot.dynamic.position.value) robot.taskUpperBody.feature.state.value = q robot.taskUpperBody.feature.posture.value = q # robotDim = robot.dynamic.getDimension() # 38 robot.taskUpperBody.feature.selectDof(18, True) robot.taskUpperBody.feature.selectDof(19, True) robot.taskUpperBody.feature.selectDof(20, True) robot.taskUpperBody.feature.selectDof(21, True) robot.taskUpperBody.feature.selectDof(22, True) robot.taskUpperBody.feature.selectDof(23, True) robot.taskUpperBody.feature.selectDof(24, True) robot.taskUpperBody.feature.selectDof(25, True) robot.taskUpperBody.feature.selectDof(26, True) robot.taskUpperBody.feature.selectDof(27, True) robot.taskUpperBody.feature.selectDof(28, True) robot.taskUpperBody.feature.selectDof(29, True) robot.taskUpperBody.feature.selectDof(30, True) robot.taskUpperBody.feature.selectDof(31, True) robot.taskUpperBody.feature.selectDof(32, True) robot.taskUpperBody.feature.selectDof(33, True) robot.taskUpperBody.feature.selectDof(34, True) robot.taskUpperBody.feature.selectDof(35, True) robot.taskUpperBody.feature.selectDof(36, True) robot.taskUpperBody.feature.selectDof(37, True) robot.taskUpperBody.controlGain.value = 100.0 robot.taskUpperBody.add(robot.taskUpperBody.feature.name) plug(robot.dynamic.position, robot.taskUpperBody.feature.state) # --- CONTACTS #define contactLF and contactRF robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle']) robot.contactLF.feature.frame('desired') robot.contactLF.gain.setConstant(300) plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) #.errorIN? locals()['contactLF'] = robot.contactLF robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) robot.contactRF.feature.frame('desired') robot.contactRF.gain.setConstant(300) plug(robot.wp.footRightDes, robot.contactRF.featureDes.position) #.errorIN? locals()['contactRF'] = robot.contactRF # --- COM height robot.taskComH = MetaTaskKineCom(robot.dynamic, name='comH') plug(robot.wp.comDes, robot.taskComH.featureDes.errorIN) robot.taskComH.task.controlGain.value = 100. robot.taskComH.feature.selec.value = '100' # --- COM robot.taskCom = MetaTaskKineCom(robot.dynamic) plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN) plug(robot.com_admittance_control.dcomRef, robot.taskCom.featureDes.errordotIN) robot.taskCom.task.controlGain.value = 0 robot.taskCom.task.setWithDerivative(True) robot.taskCom.feature.selec.value = '011' # --- Waist robot.keepWaist = MetaTaskKine6d('keepWaist', robot.dynamic, 'WT', robot.OperationalPointsMap['waist']) robot.keepWaist.feature.frame('desired') robot.keepWaist.gain.setConstant(300) plug(robot.wp.waistDes, robot.keepWaist.featureDes.position) robot.keepWaist.feature.selec.value = '111000' locals()['keepWaist'] = robot.keepWaist # --- SOT solver robot.sot = SOT('sot') robot.sot.setSize(robot.dynamic.getDimension()) # --- Plug SOT control to device through control manager plug(robot.sot.control, robot.cm.ctrl_sot_input) plug(robot.cm.u_safe, robot.device.control) robot.sot.push(robot.taskUpperBody.name) robot.sot.push(robot.contactRF.task.name) robot.sot.push(robot.contactLF.task.name) robot.sot.push(robot.taskComH.task.name) robot.sot.push(robot.taskCom.task.name) robot.sot.push(robot.keepWaist.task.name) # --- Fix robot.dynamic inputs plug(robot.device.velocity, robot.dynamic.velocity) robot.dvdt = Derivator_of_Vector("dv_dt") robot.dvdt.dt.value = dt plug(robot.device.velocity, robot.dvdt.sin) plug(robot.dvdt.sout, robot.dynamic.acceleration) # -------------------------- PLOTS -------------------------- # --- ROS PUBLISHER robot.publisher = create_rospublish(robot, 'robot_publisher') create_topic(robot.publisher, robot.device, 'state', robot=robot, data_type='vector') create_topic(robot.publisher, robot.base_estimator, 'q', robot=robot, data_type='vector') #create_topic(robot.publisher, robot.stf, 'q', robot = robot, data_type='vector') create_topic(robot.publisher, robot.comTrajGen, 'x', robot=robot, data_type='vector') # generated CoM create_topic(robot.publisher, robot.comTrajGen, 'dx', robot=robot, data_type='vector') # generated CoM velocity create_topic(robot.publisher, robot.comTrajGen, 'ddx', robot=robot, data_type='vector') # generated CoM acceleration create_topic(robot.publisher, robot.wp, 'comDes', robot=robot, data_type='vector') # desired CoM create_topic(robot.publisher, robot.cdc_estimator, 'c', robot=robot, data_type='vector') # estimated CoM create_topic(robot.publisher, robot.cdc_estimator, 'dc', robot=robot, data_type='vector') # estimated CoM velocity create_topic(robot.publisher, robot.com_admittance_control, 'comRef', robot=robot, data_type='vector') # reference CoM create_topic(robot.publisher, robot.dynamic, 'com', robot=robot, data_type='vector') # resulting SOT CoM create_topic(robot.publisher, robot.dcm_control, 'dcmDes', robot=robot, data_type='vector') # desired DCM create_topic(robot.publisher, robot.estimator, 'dcm', robot=robot, data_type='vector') # estimated DCM create_topic(robot.publisher, robot.zmpTrajGen, 'x', robot=robot, data_type='vector') # generated ZMP create_topic(robot.publisher, robot.wp, 'zmpDes', robot=robot, data_type='vector') # desired ZMP create_topic(robot.publisher, robot.dynamic, 'zmp', robot=robot, data_type='vector') # SOT ZMP create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot=robot, data_type='vector') # estimated ZMP create_topic(robot.publisher, robot.dcm_control, 'zmpRef', robot=robot, data_type='vector') # reference ZMP create_topic(robot.publisher, robot.waistTrajGen, 'x', robot=robot, data_type='vector') # desired waist orientation create_topic(robot.publisher, robot.lfTrajGen, 'x', robot=robot, data_type='vector') # desired left foot pose create_topic(robot.publisher, robot.rfTrajGen, 'x', robot=robot, data_type='vector') # desired right foot pose create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot, data_type='vector') # calibrated left wrench create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot, data_type='vector') # calibrated right wrench create_topic(robot.publisher, robot.dynamic, 'LF', robot=robot, data_type='matrixHomo') # left foot create_topic(robot.publisher, robot.dynamic, 'RF', robot=robot, data_type='matrixHomo') # right foot # --- TRACER robot.tracer = TracerRealTime("com_tracer") robot.tracer.setBufferSize(80 * (2**20)) robot.tracer.open('/tmp', 'dg_', '.dat') robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name)) addTrace(robot.tracer, robot.wp, 'comDes') # desired CoM addTrace(robot.tracer, robot.cdc_estimator, 'c') # estimated CoM addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity addTrace(robot.tracer, robot.com_admittance_control, 'comRef') # reference CoM addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM addTrace(robot.tracer, robot.dcm_control, 'zmpDes') # desired ZMP addTrace(robot.tracer, robot.dynamic, 'zmp') # SOT ZMP addTrace(robot.tracer, robot.zmp_estimator, 'zmp') # estimated ZMP addTrace(robot.tracer, robot.dcm_control, 'zmpRef') # reference ZMP addTrace(robot.tracer, robot.ftc, 'left_foot_force_out') # calibrated left wrench addTrace(robot.tracer, robot.ftc, 'right_foot_force_out') # calibrated right wrench addTrace(robot.tracer, robot.dynamic, 'LF') # left foot addTrace(robot.tracer, robot.dynamic, 'RF') # right foot robot.tracer.start()
def main(robot): robot.timeStep = robot.device.getTimeStep() dt = robot.timeStep robot.comTrajGen = create_com_trajectory_generator(dt, robot) # --- COM robot.taskCom = MetaTaskKineCom(robot.dynamic) robot.dynamic.com.recompute(0) robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value robot.taskCom.task.controlGain.value = 10 # --- CONTACTS # define contactLF and contactRF robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle']) robot.contactLF.feature.frame('desired') robot.contactLF.gain.setConstant(100) robot.contactLF.keep() locals()['contactLF'] = robot.contactLF robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) robot.contactRF.feature.frame('desired') robot.contactRF.gain.setConstant(100) robot.contactRF.keep() locals()['contactRF'] = robot.contactRF # --- SOT robot.sot = SOT('sot') robot.sot.setSize(robot.dynamic.getDimension()) plug(robot.sot.control, robot.device.control) robot.sot.push(robot.contactRF.task.name) robot.sot.push(robot.taskCom.task.name) robot.sot.push(robot.contactLF.task.name) robot.device.control.recompute(0) # --- ESTIMATION robot.param_server = create_parameter_server(param_server_conf, dt) # robot.imu_offset_compensation = create_imu_offset_compensation(robot, dt) robot.device_filters = create_device_filters(robot, dt) robot.imu_filters = create_imu_filters(robot, dt) robot.base_estimator = create_base_estimator(robot, dt, conf) robot.be_filters = create_be_filters(robot, dt) # --- TRACERS outputs = ['robotState'] robot.device_tracer = create_tracer(robot, robot.device, 'device_tracer', outputs) outputs = ['q'] robot.estimator_tracer = create_tracer(robot, robot.base_estimator, 'estimator_tracer', outputs) robot.device.after.addSignal('base_estimator.q') # --- RUN SIMULATION plug(robot.comTrajGen.x, robot.taskCom.featureDes.errorIN) sleep(1.0) os.system("rosservice call \start_dynamic_graph") sleep(2.0) robot.comTrajGen.move(1, -0.025, 4.0) sleep(5.0) robot.comTrajGen.startSinusoid(1, 0.05, 8.0) sleep(0.2) robot.device_tracer.start() robot.estimator_tracer.start() sleep(3.0) dump_tracer(robot.device_tracer) dump_tracer(robot.estimator_tracer) print('data dumped') # --- DISPLAY device_data = read_tracer_file('/tmp/dg_' + robot.device.name + '-robotState.dat') estimator_data = read_tracer_file('/tmp/dg_' + robot.base_estimator.name + '-q.dat') plot_select_traj(device_data, [10, 23, 15]) plot_select_traj(estimator_data, [10, 23, 15]) write_pdf_graph('/tmp/')
def init_online_walking(robot): # 09.04.20 est a 100 dans dcmZmpControl_file, used to be 10 cm_conf.CTRL_MAX = 1000.0 # temporary hack dt = robot.device.getTimeStep() robot.timeStep = dt # --- Pendulum parameters robot_name = 'robot' robot.dynamic.com.recompute(0) robotDim = robot.dynamic.getDimension() mass = robot.dynamic.data.mass[0] h = robot.dynamic.com.value[2] g = 9.81 omega = sqrt(g / h) # --- Parameter server robot.param_server = create_parameter_server(param_server_conf, dt) # --- Initial feet and waist robot.dynamic.createOpPoint('LF', robot.OperationalPointsMap['left-ankle']) robot.dynamic.createOpPoint('RF', robot.OperationalPointsMap['right-ankle']) robot.dynamic.createOpPoint('WT', robot.OperationalPointsMap['waist']) robot.dynamic.LF.recompute(0) robot.dynamic.RF.recompute(0) robot.dynamic.WT.recompute(0) # -------------------------- DESIRED TRAJECTORY -------------------------- rospack = RosPack() # -------------------------- PATTERN GENERATOR -------------------------- robot.pg = PatternGenerator('pg') # MODIFIED WITH MY PATHS talos_data_folder = rospack.get_path('talos_data') robot.pg.setURDFpath(talos_data_folder + '/urdf/talos_reduced_wpg.urdf') robot.pg.setSRDFpath(talos_data_folder + '/srdf/talos_wpg.srdf') ## END MODIFIED robot.pg.buildModel() robot.pg.parseCmd(":samplingperiod 0.005") robot.pg.parseCmd(":previewcontroltime 1.6") robot.pg.parseCmd(":omega 0.0") robot.pg.parseCmd(':stepheight 0.05') robot.pg.parseCmd(':doublesupporttime 0.2') robot.pg.parseCmd(':singlesupporttime 1.0') robot.pg.parseCmd(":armparameters 0.5") robot.pg.parseCmd(":LimitsFeasibility 0.0") robot.pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015") robot.pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.7 3.0") robot.pg.parseCmd(":UpperBodyMotionParameters -0.1 -1.0 0.0") robot.pg.parseCmd(":comheight 0.876681") robot.pg.parseCmd(":setVelReference 0.1 0.0 0.0") robot.pg.parseCmd(":SetAlgoForZmpTrajectory Naveau") plug(robot.dynamic.position, robot.pg.position) plug(robot.dynamic.com, robot.pg.com) #plug(robot.dynamic.com, robot.pg.comStateSIN) plug(robot.dynamic.LF, robot.pg.leftfootcurrentpos) plug(robot.dynamic.RF, robot.pg.rightfootcurrentpos) robotDim = len(robot.dynamic.velocity.value) robot.pg.motorcontrol.value = robotDim * (0, ) robot.pg.zmppreviouscontroller.value = (0, 0, 0) robot.pg.initState() robot.pg.parseCmd(':setDSFeetDistance 0.162') robot.pg.parseCmd(':NaveauOnline') robot.pg.parseCmd(':numberstepsbeforestop 2') robot.pg.parseCmd(':setfeetconstraint XY 0.091 0.0489') robot.pg.parseCmd(':deleteallobstacles') robot.pg.parseCmd(':feedBackControl false') robot.pg.parseCmd(':useDynamicFilter true') robot.pg.velocitydes.value = (0.1, 0.0, 0.0) # DEFAULT VALUE (0.1,0.0,0.0) # -------------------------- TRIGGER -------------------------- robot.triggerPG = BooleanIdentity('triggerPG') robot.triggerPG.sin.value = 0 plug(robot.triggerPG.sout, robot.pg.trigger) # --------- Interface with controller entities ------------- wp = DummyWalkingPatternGenerator('dummy_wp') wp.init() # #wp.displaySignals() wp.omega.value = omega # 22.04 after modifying pg.cpp, new way to try and connect the waist plug(robot.pg.waistattitudematrixabsolute, wp.waist) plug(robot.pg.leftfootref, wp.footLeft) plug(robot.pg.rightfootref, wp.footRight) plug(robot.pg.comref, wp.com) plug(robot.pg.dcomref, wp.vcom) plug(robot.pg.ddcomref, wp.acom) robot.wp = wp # --- Compute the values to use them in initialization robot.wp.comDes.recompute(0) robot.wp.dcmDes.recompute(0) robot.wp.zmpDes.recompute(0) ## END ADDED # -------------------------- ESTIMATION -------------------------- # --- Base Estimation robot.device_filters = create_device_filters(robot, dt) robot.imu_filters = create_imu_filters(robot, dt) robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf) robot.m2qLF = MatrixHomoToPoseQuaternion('m2qLF') plug(robot.dynamic.LF, robot.m2qLF.sin) plug(robot.m2qLF.sout, robot.base_estimator.lf_ref_xyzquat) robot.m2qRF = MatrixHomoToPoseQuaternion('m2qRF') plug(robot.dynamic.RF, robot.m2qRF.sin) plug(robot.m2qRF.sout, robot.base_estimator.rf_ref_xyzquat) # --- Conversion e2q = EulerToQuat('e2q') plug(robot.base_estimator.q, e2q.euler) robot.e2q = e2q # --- Kinematic computations robot.rdynamic = DynamicPinocchio("real_dynamics") robot.rdynamic.setModel(robot.dynamic.model) robot.rdynamic.setData(robot.rdynamic.model.createData()) plug(robot.base_estimator.q, robot.rdynamic.position) robot.rdynamic.velocity.value = [0.0] * robotDim robot.rdynamic.acceleration.value = [0.0] * robotDim # --- CoM Estimation cdc_estimator = DcmEstimator('cdc_estimator') cdc_estimator.init(dt, robot_name) plug(robot.e2q.quaternion, cdc_estimator.q) plug(robot.base_estimator.v, cdc_estimator.v) robot.cdc_estimator = cdc_estimator # --- DCM Estimation estimator = DummyDcmEstimator("dummy") estimator.omega.value = omega estimator.mass.value = 1.0 plug(robot.cdc_estimator.c, estimator.com) plug(robot.cdc_estimator.dc, estimator.momenta) estimator.init() robot.estimator = estimator # --- Force calibration robot.ftc = create_ft_calibrator(robot, ft_conf) # --- ZMP estimation zmp_estimator = SimpleZmpEstimator("zmpEst") robot.rdynamic.createOpPoint('sole_LF', 'left_sole_link') robot.rdynamic.createOpPoint('sole_RF', 'right_sole_link') plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft) plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight) plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft) plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight) zmp_estimator.init() robot.zmp_estimator = zmp_estimator # -------------------------- ADMITTANCE CONTROL -------------------------- # --- DCM controller Kp_dcm = [8.0] * 3 Ki_dcm = [0.0, 0.0, 0.0] # zero (to be set later) gamma_dcm = 0.2 dcm_controller = DcmController("dcmCtrl") dcm_controller.Kp.value = Kp_dcm dcm_controller.Ki.value = Ki_dcm dcm_controller.decayFactor.value = gamma_dcm dcm_controller.mass.value = mass dcm_controller.omega.value = omega plug(robot.cdc_estimator.c, dcm_controller.com) plug(robot.estimator.dcm, dcm_controller.dcm) plug(robot.wp.zmpDes, dcm_controller.zmpDes) plug(robot.wp.dcmDes, dcm_controller.dcmDes) dcm_controller.init(dt) robot.dcm_control = dcm_controller Ki_dcm = [1.0, 1.0, 1.0] # this value is employed later # --- CoM admittance controller Kp_adm = [0.0, 0.0, 0.0] # zero (to be set later) com_admittance_control = ComAdmittanceController("comAdmCtrl") com_admittance_control.Kp.value = Kp_adm plug(robot.zmp_estimator.zmp, com_admittance_control.zmp) com_admittance_control.zmpDes.value = robot.wp.zmpDes.value # should be plugged to robot.dcm_control.zmpRef plug(robot.wp.acomDes, com_admittance_control.ddcomDes) com_admittance_control.init(dt) com_admittance_control.setState(robot.wp.comDes.value, [0.0, 0.0, 0.0]) robot.com_admittance_control = com_admittance_control Kp_adm = [15.0, 15.0, 0.0] # this value is employed later # --- Control Manager robot.cm = create_ctrl_manager(cm_conf, dt, robot_name='robot') robot.cm.addCtrlMode('sot_input') robot.cm.setCtrlMode('all', 'sot_input') robot.cm.addEmergencyStopSIN('zmp') # -------------------------- SOT CONTROL -------------------------- # --- Upper body robot.taskUpperBody = Task('task_upper_body') robot.taskUpperBody.feature = FeaturePosture('feature_upper_body') q = list(robot.dynamic.position.value) robot.taskUpperBody.feature.state.value = q robot.taskUpperBody.feature.posture.value = q robot.taskUpperBody.feature.selectDof(18, True) robot.taskUpperBody.feature.selectDof(19, True) robot.taskUpperBody.feature.selectDof(20, True) robot.taskUpperBody.feature.selectDof(21, True) robot.taskUpperBody.feature.selectDof(22, True) robot.taskUpperBody.feature.selectDof(23, True) robot.taskUpperBody.feature.selectDof(24, True) robot.taskUpperBody.feature.selectDof(25, True) robot.taskUpperBody.feature.selectDof(26, True) robot.taskUpperBody.feature.selectDof(27, True) robot.taskUpperBody.feature.selectDof(28, True) robot.taskUpperBody.feature.selectDof(29, True) robot.taskUpperBody.feature.selectDof(30, True) robot.taskUpperBody.feature.selectDof(31, True) robot.taskUpperBody.feature.selectDof(32, True) robot.taskUpperBody.feature.selectDof(33, True) robot.taskUpperBody.feature.selectDof(34, True) robot.taskUpperBody.feature.selectDof(35, True) robot.taskUpperBody.feature.selectDof(36, True) robot.taskUpperBody.feature.selectDof(37, True) robot.taskUpperBody.controlGain.value = 100.0 robot.taskUpperBody.add(robot.taskUpperBody.feature.name) plug(robot.dynamic.position, robot.taskUpperBody.feature.state) # --- CONTACTS robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle']) robot.contactLF.feature.frame('desired') robot.contactLF.gain.setConstant(300) plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) #.errorIN? locals()['contactLF'] = robot.contactLF robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) robot.contactRF.feature.frame('desired') robot.contactRF.gain.setConstant(300) plug(robot.wp.footRightDes, robot.contactRF.featureDes.position) #.errorIN? locals()['contactRF'] = robot.contactRF # --- COM height robot.taskComH = MetaTaskKineCom(robot.dynamic, name='comH') plug(robot.wp.comDes, robot.taskComH.featureDes.errorIN) robot.taskComH.task.controlGain.value = 100. robot.taskComH.feature.selec.value = '100' # --- COM robot.taskCom = MetaTaskKineCom(robot.dynamic) plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN) plug(robot.com_admittance_control.dcomRef, robot.taskCom.featureDes.errordotIN) robot.taskCom.task.controlGain.value = 0 robot.taskCom.task.setWithDerivative(True) robot.taskCom.feature.selec.value = '011' # --- Waist robot.keepWaist = MetaTaskKine6d('keepWaist', robot.dynamic, 'WT', robot.OperationalPointsMap['waist']) robot.keepWaist.feature.frame('desired') robot.keepWaist.gain.setConstant(300) plug(robot.wp.waistDes, robot.keepWaist.featureDes.position) #de base robot.keepWaist.feature.selec.value = '111000' locals()['keepWaist'] = robot.keepWaist # --- SOT solver robot.sot = SOT('sot') robot.sot.setSize(robot.dynamic.getDimension()) # --- Plug SOT control to device through control manager plug(robot.sot.control, robot.cm.ctrl_sot_input) plug(robot.cm.u_safe, robot.device.control) robot.sot.push(robot.taskUpperBody.name) robot.sot.push(robot.contactRF.task.name) robot.sot.push(robot.contactLF.task.name) robot.sot.push(robot.taskComH.task.name) robot.sot.push(robot.taskCom.task.name) robot.sot.push(robot.keepWaist.task.name) # --- Fix robot.dynamic inputs plug(robot.device.velocity, robot.dynamic.velocity) robot.dvdt = Derivator_of_Vector("dv_dt") robot.dvdt.dt.value = dt plug(robot.device.velocity, robot.dvdt.sin) plug(robot.dvdt.sout, robot.dynamic.acceleration) # -------------------------- PLOTS -------------------------- # --- ROS PUBLISHER ## THIS PARAGRAPH QUITE DIFFERENT, TO CHECK robot.publisher = create_rospublish(robot, 'robot_publisher') ## ADDED create_topic(robot.publisher, robot.pg, 'comref', robot=robot, data_type='vector') # desired CoM create_topic(robot.publisher, robot.pg, 'dcomref', robot=robot, data_type='vector') create_topic(robot.publisher, robot.wp, 'waist', robot=robot, data_type='matrixHomo') create_topic(robot.publisher, robot.keepWaist.featureDes, 'position', robot=robot, data_type='matrixHomo') create_topic(robot.publisher, robot.dynamic, 'WT', robot=robot, data_type='matrixHomo') create_topic(robot.publisher, robot.pg, 'waistattitudematrixabsolute', robot=robot, data_type='matrixHomo') ## que font ces lignes exactement ?? create_topic(robot.publisher, robot.pg, 'leftfootref', robot=robot, data_type='matrixHomo') create_topic(robot.publisher, robot.wp, 'footLeft', robot=robot, data_type='matrixHomo') create_topic(robot.publisher, robot.pg, 'rightfootref', robot=robot, data_type='matrixHomo') create_topic(robot.publisher, robot.wp, 'footRight', robot=robot, data_type='matrixHomo') ## --- TRACER robot.tracer = TracerRealTime("com_tracer") robot.tracer.setBufferSize(80 * (2**20)) robot.tracer.open('/tmp', 'dg_', '.dat') robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name)) addTrace(robot.tracer, robot.pg, 'waistattitudeabsolute') # fin addTrace(robot.tracer, robot.wp, 'comDes') # desired CoM addTrace(robot.tracer, robot.cdc_estimator, 'c') # estimated CoM addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity addTrace(robot.tracer, robot.pg, 'comref') addTrace(robot.tracer, robot.pg, 'dcomref') addTrace(robot.tracer, robot.pg, 'ddcomref') addTrace(robot.tracer, robot.pg, 'rightfootref') addTrace(robot.tracer, robot.pg, 'leftfootref') addTrace(robot.tracer, robot.pg, 'rightfootcontact') addTrace(robot.tracer, robot.pg, 'leftfootcontact') addTrace(robot.tracer, robot.pg, 'SupportFoot') addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM addTrace(robot.tracer, robot.dynamic, 'LF') # left foot addTrace(robot.tracer, robot.dynamic, 'RF') # right foot robot.tracer.start()
q = list(robot.dynamic.position.value) robot.taskUpperBody.feature.state.value = q robot.taskUpperBody.feature.posture.value = q robotDim = robot.dynamic.getDimension() # 38 for i in range(18, robotDim): robot.taskUpperBody.feature.selectDof(i, True) robot.taskUpperBody.controlGain.value = 100.0 robot.taskUpperBody.add(robot.taskUpperBody.feature.name) plug(robot.dynamic.position, robot.taskUpperBody.feature.state) # --- CONTACTS #define contactLF and contactRF robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle']) robot.contactLF.feature.frame('desired') robot.contactLF.gain.setConstant(300) plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) #.errorIN? locals()['contactLF'] = robot.contactLF robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) robot.contactRF.feature.frame('desired') robot.contactRF.gain.setConstant(300) plug(robot.wp.footRightDes, robot.contactRF.featureDes.position) #.errorIN? locals()['contactRF'] = robot.contactRF # --- COM height robot.taskComH = MetaTaskKineCom(robot.dynamic, name='comH') plug(robot.wp.comDes, robot.taskComH.featureDes.errorIN)
plug(sot.control, robot.device.control) from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts dt = 1e-3 @loopInThread def inc(): robot.device.increment(dt) runner = inc() [go, stop, next, n] = loopShortcuts(runner) # Tasks taskzero = MetaTaskKine6d('rh', robot.dynamic, 'rh', 'right-wrist') taskzero.feature.frame('desired') robot.dynamic.upperJl.recompute(0) robot.dynamic.lowerJl.recompute(0) taskJL = TaskJointLimits('taskJL') plug(robot.dynamic.position, taskJL.position) taskJL.controlGain.value = 10 taskJL.referenceInf.value = robot.dynamic.lowerJl.value taskJL.referenceSup.value = robot.dynamic.upperJl.value taskJL.dt.value = dt taskJL.selec.value = toFlags( range(18, 25) + range(26, 27) + range(28, 31) + range(32, 40) + range(41, 42) + range(43, 46) + range(47, 50)) taskContact = MetaTaskKine6d('contact', robot.dynamic, 'contact', 'left-ankle')