def add_motion_task(self, kp=50, kd=1000, w=1.0): pos = self.robot.position(self.invdyn.data(), self.robot.model().getJointId(self.name)) self.motion_traj = tsid.TrajectorySE3Constant(self.name + "_traj", pos) self.motion_ref = self.motion_traj.computeNext() self.motion_task = tsid.TaskSE3Equality(self.name + "_motion", self.robot, self.name) self.motion_task.setKp(kp * np.matrix(np.ones(6)).transpose()) self.motion_task.setKp(kd * np.matrix(np.ones(6)).transpose()) self.invdyn.addMotionTask(self.motion_task, w, 1, 0.0)
comTask.setKp(kp_com * np.matrix(np.ones(3)).transpose()) comTask.setKd(2.0 * np.sqrt(kp_com) * np.matrix(np.ones(3)).transpose()) invdyn.addMotionTask(comTask, w_com, 1, 0.0) postureTask = tsid.TaskJointPosture("task-posture", robot) postureTask.setKp(kp_posture * np.matrix(np.ones(robot.nv-6)).transpose()) postureTask.setKd(2.0 * np.sqrt(kp_posture) * np.matrix(np.ones(robot.nv-6)).transpose()) invdyn.addMotionTask(postureTask, w_posture, 1, 0.0) rightFootTask = tsid.TaskSE3Equality("task-right-foot", robot, rf_frame_name) rightFootTask.setKp(kp_RF * np.matrix(np.ones(6)).transpose()) rightFootTask.setKd(2.0 * np.sqrt(kp_com) * np.matrix(np.ones(6)).transpose()) invdyn.addMotionTask(rightFootTask, w_RF, 1, 0.0) H_rf_ref.translation += np.matrix([0., 0., DELTA_FOOT_Z]).T rightFootTraj = tsid.TrajectorySE3Constant("traj-right-foot", H_rf_ref) com_ref = robot.com(data) com_ref[1] += DELTA_COM_Y trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref) q_ref = q[7:] trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref) solver = tsid.SolverHQuadProg("qp solver") solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn) for i in range(0, N_SIMULATION): time_start = time.time() if i == REMOVE_CONTACT_N:
zGoal = tbase[2,0] - 0.235 # match with the ref config of the feet FR_foot_goal = FR_foot_ref.copy() FR_foot_goal.translation = np.matrix([tFR[0,0], tFR[1,0], zGoal]).T FL_foot_goal = FL_foot_ref.copy() FL_foot_goal.translation = np.matrix([tFL[0,0], tFL[1,0], zGoal]).T HR_foot_goal = HR_foot_ref.copy() HR_foot_goal.translation = np.matrix([tHR[0,0], tHR[1,0], zGoal]).T HL_foot_goal = HL_foot_ref.copy() HL_foot_goal.translation = np.matrix([tHL[0,0], tHL[1,0], zGoal]).T trajHLfoot = tsid.TrajectorySE3Constant("traj_HL_foot", HL_foot_goal) trajHRfoot = tsid.TrajectorySE3Constant("traj_HR_foot", HR_foot_goal) trajFLfoot = tsid.TrajectorySE3Constant("traj_FL_foot", FL_foot_goal) trajFRfoot = tsid.TrajectorySE3Constant("traj_FR_foot", FR_foot_goal) samplePosture = trajPosture.computeNext() postureTask.setReference(samplePosture) sampleHLfoot = trajHLfoot.computeNext() HLfootTask.setReference(sampleHLfoot) sampleHRfoot = trajHRfoot.computeNext()
def __init__(self, conf, viewer=True): self.conf = conf self.contact_frame_names = conf.contact_frame_names print('Robot files:') print(conf.urdf) print(conf.srdf) vector = pin.StdVec_StdString() vector.extend(item for item in conf.path) self.robot = tsid.RobotWrapper(conf.urdf, vector, pin.JointModelFreeFlyer(), False) robot = self.robot self.model = robot.model() pin.loadReferenceConfigurations(self.model, conf.srdf, False) self.q0 = q = self.model.referenceConfigurations[ conf.reference_config_q_name] v = np.zeros(robot.nv) assert (all([ self.model.existFrame(frame_name) for frame_name in self.contact_frame_names ])) invdyn = tsid.InverseDynamicsFormulationAccForce('tsid', robot, False) invdyn.computeProblemData(0.0, q, v) data = invdyn.data() robot.computeAllTerms(data, q, v) ##################################### # define contact and associated tasks ##################################### self.contact_frame_ids = [ self.model.getFrameId(frame_name) for frame_name in self.contact_frame_names ] self.nc = len(self.contact_frame_ids) # contact number self.contacts = self.nc * [None] self.tasks_tracking_foot = self.nc * [None] self.traj_feet = self.nc * [None] self.sample_feet = self.nc * [None] mask = np.ones(6) if not conf.contact6d: mask[3:] = 0 for i_foot, (frame_name, fid) in enumerate( zip(self.contact_frame_names, self.contact_frame_ids)): Hf_ref = robot.framePosition(data, fid) if conf.contact6d: # DEFINE FOOT CONTACT POINTS LOCATION WRT LOCAL FOOT FRAMES # contact_points = np.ones((3,4)) * conf.lz contact_points = -np.ones((3, 4)) * conf.lz contact_points[0, :] = [ -conf.lxn, -conf.lxn, conf.lxp, conf.lxp ] contact_points[1, :] = [ -conf.lyn, conf.lyp, -conf.lyn, conf.lyp ] # RIGID CONTACT RIGHT FOOT self.contacts[i_foot] = tsid.Contact6d( 'contact_{}'.format(frame_name), robot, frame_name, contact_points, conf.contactNormal, conf.mu, conf.fMin, conf.fMax) self.contacts[i_foot].setKp(conf.kp_contact * np.ones(6).T) self.contacts[i_foot].setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6).T) self.contacts[i_foot].setReference(Hf_ref) invdyn.addRigidContact(self.contacts[i_foot], conf.w_forceRef) else: # RIGID POINT CONTACTS self.contacts[i_foot] = tsid.ContactPoint( 'contact_{}'.format(frame_name), robot, frame_name, conf.contactNormal, conf.mu, conf.fMin, conf.fMax) self.contacts[i_foot].setKp(conf.kp_contact * np.ones(3)) self.contacts[i_foot].setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(3)) self.contacts[i_foot].setReference(Hf_ref) self.contacts[i_foot].useLocalFrame(conf.useLocalFrame) invdyn.addRigidContact(self.contacts[i_foot], conf.w_forceRef) # FEET TRACKING TASKS self.tasks_tracking_foot[i_foot] = tsid.TaskSE3Equality( 'task-foot{}'.format(i_foot), self.robot, self.contact_frame_names[i_foot]) self.tasks_tracking_foot[i_foot].setKp(conf.kp_foot * mask) self.tasks_tracking_foot[i_foot].setKd( 2.0 * np.sqrt(conf.kp_foot) * mask) self.tasks_tracking_foot[i_foot].setMask(mask) self.tasks_tracking_foot[i_foot].useLocalFrame(False) invdyn.addMotionTask(self.tasks_tracking_foot[i_foot], conf.w_foot, conf.priority_foot, 0.0) self.traj_feet[i_foot] = tsid.TrajectorySE3Constant( 'traj-foot{}'.format(i_foot), Hf_ref) self.sample_feet[i_foot] = self.traj_feet[i_foot].computeNext() ############# # Other tasks ############# # COM TASK self.comTask = tsid.TaskComEquality('task-com', robot) self.comTask.setKp(conf.kp_com * np.ones(3)) self.comTask.setKd(2.0 * np.sqrt(conf.kp_com) * np.ones(3)) invdyn.addMotionTask(self.comTask, conf.w_com, conf.priority_com, 1.0) # POSTURE TASK self.postureTask = tsid.TaskJointPosture('task-posture', robot) self.postureTask.setKp(conf.kp_posture * np.ones(robot.nv - 6)) self.postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) * np.ones(robot.nv - 6)) invdyn.addMotionTask(self.postureTask, conf.w_posture, conf.priority_posture, 0.0) # TRUNK Task self.trunkTask = tsid.TaskSE3Equality("keepTrunk", robot, 'root_joint') self.trunkTask.setKp(conf.kp_trunk * np.ones(6)) self.trunkTask.setKd(2.0 * np.sqrt(conf.kp_trunk) * np.ones(6)) # Add a Mask to the task which will select the vector dimensions on which the task will act. # In this case the trunk configuration is a vector 6d (position and orientation -> SE3) # Here we set a mask = [0 0 0 1 1 1] so the task on the trunk will act on the orientation of the robot mask = np.ones(6) mask[:3] = 0. self.trunkTask.useLocalFrame(False) self.trunkTask.setMask(mask) invdyn.addMotionTask(self.trunkTask, conf.w_trunk, conf.priority_trunk, 0.0) # TORQUE BOUND TASK # self.tau_max = conf.tau_max_scaling*robot.model().effortLimit[-robot.na:] # self.tau_min = -self.tau_max # actuationBoundsTask = tsid.TaskActuationBounds('task-actuation-bounds', robot) # actuationBoundsTask.setBounds(self.tau_min, self.tau_max) # if(conf.w_torque_bounds>0.0): # invdyn.addActuationTask(actuationBoundsTask, conf.w_torque_bounds, conf.priority_torque_bounds, 0.0) # JOINT BOUND TASK # jointBoundsTask = tsid.TaskJointBounds('task-joint-bounds', robot, conf.dt) # self.v_max = conf.v_max_scaling * robot.model().velocityLimit[-robot.na:] # self.v_min = -self.v_max # jointBoundsTask.setVelocityBounds(self.v_min, self.v_max) # if(conf.w_joint_bounds>0.0): # invdyn.addMotionTask(jointBoundsTask, conf.w_joint_bounds, conf.priority_joint_bounds, 0.0) ################################## # Init task reference trajectories ################################## self.sample_feet = self.nc * [None] self.sample_feet_pos = self.nc * [None] self.sample_feet_vel = self.nc * [None] self.sample_feet_acc = self.nc * [None] for i_foot, traj in enumerate(self.traj_feet): self.sample_feet[i_foot] = traj.computeNext() self.sample_feet_pos[i_foot] = self.sample_feet[i_foot].pos() self.sample_feet_vel[i_foot] = self.sample_feet[i_foot].vel() self.sample_feet_acc[i_foot] = self.sample_feet[i_foot].acc() com_ref = robot.com(data) self.trajCom = tsid.TrajectoryEuclidianConstant('traj_com', com_ref) self.sample_com = self.trajCom.computeNext() q_ref = q[7:] self.trajPosture = tsid.TrajectoryEuclidianConstant( 'traj_joint', q_ref) self.postureTask.setReference(self.trajPosture.computeNext()) self.trunk_link_id = self.model.getFrameId('base_link') R_trunk_init = robot.framePosition(data, self.trunk_link_id).rotation self.trunk_ref = self.robot.framePosition(data, self.trunk_link_id) self.trajTrunk = tsid.TrajectorySE3Constant("traj_base_link", self.trunk_ref) self.sample_trunk = self.trajTrunk.computeNext() pos_trunk = np.hstack([np.zeros(3), R_trunk_init.flatten()]) self.sample_trunk.pos() self.sample_trunk.vel(np.zeros(6)) self.sample_trunk.acc(np.zeros(6)) self.trunkTask.setReference(self.sample_trunk) self.solver = tsid.SolverHQuadProgFast('qp solver') self.solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn) self.invdyn = invdyn self.q = q self.v = v self.active_contacts = self.nc * [True] # for gepetto viewer if (viewer): self.robot_display = pin.RobotWrapper.BuildFromURDF( conf.urdf, [ conf.path, ], pin.JointModelFreeFlyer()) l = subprocess.getstatusoutput( "ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l") if int(l[1]) == 0: os.system('gepetto-gui &') time.sleep(1) gepetto.corbaserver.Client() self.robot_display.initViewer(loadModel=True) self.robot_display.displayCollisions(False) self.robot_display.displayVisuals(True) self.robot_display.display(q) self.gui = self.robot_display.viewer.gui self.gui.setCameraTransform('python-pinocchio', conf.CAMERA_TRANSFORM) self.gui.addFloor('world/floor') self.gui.setLightingMode('world/floor', 'OFF') self.gui.addSphere('world/com', conf.SPHERE_RADIUS, conf.COM_SPHERE_COLOR) self.gui.addSphere('world/com_ref', conf.REF_SPHERE_RADIUS, conf.COM_REF_SPHERE_COLOR) for frame_name in self.contact_frame_names: self.gui.addSphere('world/{}'.format(frame_name), conf.SPHERE_RADIUS, conf.F_SPHERE_COLOR) self.gui.addSphere('world/{}_ref'.format(frame_name), conf.REF_SPHERE_RADIUS, conf.F_REF_SPHERE_COLOR)
def __init__(self, conf, viewer=pin.visualize.MeshcatVisualizer): self.conf = conf self.t_limit = False self.j_limit = True self.robot = tsid.RobotWrapper(conf.urdf, [conf.path], pin.JointModelFreeFlyer(), False) robot = self.robot self.model = robot.model() pin.loadReferenceConfigurations(self.model, conf.srdf, False) self.q0 = q = self.model.referenceConfigurations["half_sitting"] self.q0[0] = q[0] + 0.003 + 0.6 self.q0[1] = q[1] - 0.001 # self.q0[2] += 0.84 q = self.q0 v = np.zeros(robot.nv) assert self.model.existFrame(conf.rf_frame_name) assert self.model.existFrame(conf.lf_frame_name) formulation = tsid.InverseDynamicsFormulationAccForce( "tsid", robot, False) formulation.computeProblemData(0.0, q, v) data = formulation.data() contact_Point = np.ones((3, 4)) * conf.lz contact_Point[0, :] = [-conf.lxn, -conf.lxn, conf.lxp, conf.lxp] contact_Point[1, :] = [-conf.lyn, conf.lyp, -conf.lyn, conf.lyp] contactRF = tsid.Contact6d("contact_rfoot", robot, conf.rf_frame_name, contact_Point, conf.contactNormal, conf.mu, conf.fMin, conf.fMax) contactRF.setKp(conf.kp_contact * np.ones(6)) contactRF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6)) self.RF = robot.model().getFrameId(conf.rf_frame_name) H_rf_ref = robot.framePosition(data, self.RF) # modify initial robot configuration so that foot is on the ground (z=0) q[2] -= H_rf_ref.translation[2] - conf.lz formulation.computeProblemData(0.0, q, v) data = formulation.data() H_rf_ref = robot.framePosition(data, self.RF) contactRF.setReference(H_rf_ref) if conf.w_contact >= 0.0: formulation.addRigidContact(contactRF, conf.w_forceRef, conf.w_contact, 1) else: formulation.addRigidContact(contactRF, conf.w_forceRef) contactLF = tsid.Contact6d("contact_lfoot", robot, conf.lf_frame_name, contact_Point, conf.contactNormal, conf.mu, conf.fMin, conf.fMax) contactLF.setKp(conf.kp_contact * np.ones(6)) contactLF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6)) self.LF = robot.model().getFrameId(conf.lf_frame_name) H_lf_ref = robot.framePosition(data, self.LF) contactLF.setReference(H_lf_ref) if conf.w_contact >= 0.0: formulation.addRigidContact(contactLF, conf.w_forceRef, conf.w_contact, 1) else: formulation.addRigidContact(contactLF, conf.w_forceRef) comTask = tsid.TaskComEquality("task-com", robot) comTask.setKp(conf.kp_com * np.ones(3)) comTask.setKd(2.0 * np.sqrt(conf.kp_com) * np.ones(3)) formulation.addMotionTask(comTask, conf.w_com, 1, 0.0) postureTask = tsid.TaskJointPosture("task-posture", robot) postureTask.setKp(conf.kp_posture * conf.gain_vector) postureTask.setKd(2.0 * np.sqrt(conf.kp_posture * conf.gain_vector)) postureTask.mask(conf.masks_posture) formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0) orientationRootTask = tsid.TaskSE3Equality("task-orientation-root", robot, 'root_joint') mask = np.ones(6) mask[0:3] = 0 orientationRootTask.setMask(mask) orientationRootTask.setKp(conf.kp_rootOrientation * mask) orientationRootTask.setKd(2.0 * np.sqrt(conf.kp_rootOrientation * mask)) trajoriRoot = tsid.TrajectorySE3Constant("traj-ori", pin.SE3().Identity()) formulation.addMotionTask(orientationRootTask, conf.w_rootOrientation, 1, 0.0) self.amTask = tsid.TaskAMEquality("task-am", robot) self.amTask.setKp(conf.kp_am * np.array([1., 1., 0.])) self.amTask.setKd(2.0 * np.sqrt(conf.kp_am * np.array([1., 1., 0.]))) formulation.addMotionTask(self.amTask, conf.w_am, 1, 0.) self.sampleAM = tsid.TrajectorySample(3) self.amTask.setReference(self.sampleAM) self.leftFootTask = tsid.TaskSE3Equality("task-left-foot", self.robot, self.conf.lf_frame_name) self.leftFootTask.setKp(self.conf.kp_foot * np.ones(6)) self.leftFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6)) self.trajLF = tsid.TrajectorySE3Constant("traj-left-foot", H_lf_ref) formulation.addMotionTask(self.leftFootTask, self.conf.w_foot, 1, 0.0) self.rightFootTask = tsid.TaskSE3Equality("task-right-foot", self.robot, self.conf.rf_frame_name) self.rightFootTask.setKp(self.conf.kp_foot * np.ones(6)) self.rightFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6)) self.trajRF = tsid.TrajectorySE3Constant("traj-right-foot", H_rf_ref) formulation.addMotionTask(self.rightFootTask, self.conf.w_foot, 1, 0.0) if self.t_limit: self.tau_max = conf.tau_max_scaling * robot.model( ).effortLimit[-robot.na:] self.tau_min = -self.tau_max actuationBoundsTask = tsid.TaskActuationBounds( "task-actuation-bounds", robot) actuationBoundsTask.setBounds(self.tau_min, self.tau_max) if conf.w_torque_bounds > 0.0: print("torque_limit") formulation.addActuationTask(actuationBoundsTask, conf.w_torque_bounds, 0, 0.0) if self.j_limit: jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, conf.dt) self.v_max = conf.v_max_scaling * robot.model( ).velocityLimit[-robot.na:] self.v_min = -self.v_max jointBoundsTask.setVelocityBounds(self.v_min, self.v_max) if conf.w_joint_bounds > 0.0: print("joint_limit") formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, 0, 0.0) com_ref = robot.com(data) self.trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref) self.sample_com = self.trajCom.computeNext() q_ref = q[7:] q_ref[:12] = np.array([ 0., 0., -0.4114, 0.8594, -0.448, -0.0017, 0., 0., -0.4114, 0.8594, -0.448, -0.0017 ]) self.trajPosture = tsid.TrajectoryEuclidianConstant( "traj_joint", q_ref) postureTask.setReference(self.trajPosture.computeNext()) orientationRootTask.setReference(trajoriRoot.computeNext()) self.sampleLF = self.trajLF.computeNext() self.sample_LF_pos = self.sampleLF.pos() self.sample_LF_vel = self.sampleLF.vel() self.sample_LF_acc = self.sampleLF.acc() self.sampleRF = self.trajRF.computeNext() self.sample_RF_pos = self.sampleRF.pos() self.sample_RF_vel = self.sampleRF.vel() self.sample_RF_acc = self.sampleRF.acc() self.solver = tsid.SolverHQuadProgFast("qp solver") self.solver.resize(formulation.nVar, formulation.nEq, formulation.nIn) self.comTask = comTask self.postureTask = postureTask self.contactRF = contactRF self.contactLF = contactLF if self.t_limit: self.actuationBoundsTask = actuationBoundsTask if self.j_limit: self.jointBoundsTask = jointBoundsTask self.orientationRootTask = orientationRootTask self.formulation = formulation self.q = q self.v = v self.contact_LF_active = True self.contact_RF_active = True if viewer: self.robot_display = pin.RobotWrapper.BuildFromURDF( conf.urdf, [conf.path], pin.JointModelFreeFlyer()) if viewer == pin.visualize.GepettoVisualizer: import gepetto.corbaserver launched = subprocess.getstatusoutput( "ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l") if int(launched[1]) == 0: os.system('gepetto-gui &') time.sleep(1) self.viz = viewer(self.robot_display.model, self.robot_display.collision_model, self.robot_display.visual_model) self.viz.initViewer(loadModel=True) self.viz.displayCollisions(False) self.viz.displayVisuals(True) self.viz.display(q) self.gui = self.viz.viewer.gui # self.gui.setCameraTransform(0, conf.CAMERA_TRANSFORM) # self.gui.addFloor('world/floor') self.gui.setLightingMode('world/floor', 'OFF') elif viewer == pin.visualize.MeshcatVisualizer: self.viz = viewer(self.robot_display.model, self.robot_display.collision_model, self.robot_display.visual_model) self.viz.initViewer(loadModel=True) self.viz.display(q)
def __init__(self, conf, viewer=True): self.conf = conf vector = se3.StdVec_StdString() vector.extend(item for item in conf.path) self.robot = tsid.RobotWrapper(conf.urdf, vector, False) robot = self.robot self.model = model = robot.model() try: # q = se3.getNeutralConfiguration(model, conf.srdf, False) se3.loadReferenceConfigurations(model, conf.srdf, False) q = model.referenceConfigurations['default'] # q = model.referenceConfigurations["half_sitting"] except: q = conf.q0 # q = np.matrix(np.zeros(robot.nv)).T v = np.matrix(np.zeros(robot.nv)).T assert model.existFrame(conf.ee_frame_name) formulation = tsid.InverseDynamicsFormulationAccForce( "tsid", robot, False) formulation.computeProblemData(0.0, q, v) postureTask = tsid.TaskJointPosture("task-posture", robot) postureTask.setKp(conf.kp_posture * matlib.ones(robot.nv).T) postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) * matlib.ones(robot.nv).T) formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0) self.eeTask = tsid.TaskSE3Equality("task-ee", self.robot, self.conf.ee_frame_name) self.eeTask.setKp(self.conf.kp_ee * np.matrix(np.ones(6)).T) self.eeTask.setKd(2.0 * np.sqrt(self.conf.kp_ee) * np.matrix(np.ones(6)).T) self.eeTask.setMask(conf.ee_task_mask) self.eeTask.useLocalFrame(False) self.EE = model.getFrameId(conf.ee_frame_name) H_ee_ref = self.robot.framePosition(formulation.data(), self.EE) self.trajEE = tsid.TrajectorySE3Constant("traj-ee", H_ee_ref) formulation.addMotionTask(self.eeTask, conf.w_ee, 1, 0.0) self.tau_max = conf.tau_max_scaling * model.effortLimit self.tau_min = -self.tau_max actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot) actuationBoundsTask.setBounds(self.tau_min, self.tau_max) if (conf.w_torque_bounds > 0.0): formulation.addActuationTask(actuationBoundsTask, conf.w_torque_bounds, 0, 0.0) jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, conf.dt) self.v_max = conf.v_max_scaling * model.velocityLimit self.v_min = -self.v_max jointBoundsTask.setVelocityBounds(self.v_min, self.v_max) if (conf.w_joint_bounds > 0.0): formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, 0, 0.0) trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q) postureTask.setReference(trajPosture.computeNext()) solver = tsid.SolverHQuadProgFast("qp solver") solver.resize(formulation.nVar, formulation.nEq, formulation.nIn) self.trajPosture = trajPosture self.postureTask = postureTask self.actuationBoundsTask = actuationBoundsTask self.jointBoundsTask = jointBoundsTask self.formulation = formulation self.solver = solver self.q = q self.v = v # for gepetto viewer if (viewer): self.robot_display = se3.RobotWrapper.BuildFromURDF( conf.urdf, [ conf.path, ]) l = subprocess.getstatusoutput( "ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l") if int(l[1]) == 0: os.system('gepetto-gui &') time.sleep(1) gepetto.corbaserver.Client() self.robot_display.initViewer(loadModel=True) self.robot_display.displayCollisions(False) self.robot_display.displayVisuals(True) self.robot_display.display(q) self.gui = self.robot_display.viewer.gui self.gui.setCameraTransform(0, conf.CAMERA_TRANSFORM)
assert np.linalg.norm(traj_sample.pos() - q_ref, 2) < tol assert np.linalg.norm(traj_sample.vel() - zero, 2) < tol assert np.linalg.norm(traj_sample.acc() - zero, 2) < tol print "" print "Test Trajectory SE3" print "" M_ref = se3.SE3.Identity() M_vec = np.matrix(np.zeros(12)).transpose() M_vec[0:3] = M_ref.translation zero = np.matrix(np.zeros(6)).transpose() for i in range(1, 4): M_vec[3 * i:3 * i + 3] = M_ref.rotation[:, i - 1] traj_se3 = tsid.TrajectorySE3Constant("traj_se3") traj_se3.setReference(M_ref) assert traj_se3.has_trajectory_ended() assert np.linalg.norm(traj_se3.computeNext().pos() - M_vec, 2) < tol assert np.linalg.norm(traj_se3.getSample(0.0).pos() - M_vec, 2) < tol traj_sample = tsid.TrajectorySample(12, 6) traj_se3.getLastSample(traj_sample) assert np.linalg.norm(traj_sample.pos() - M_vec, 2) < tol assert np.linalg.norm(traj_sample.vel() - zero, 2) < tol assert np.linalg.norm(traj_sample.acc() - zero, 2) < tol print "All test is done"
def control(self, qtsid, vtsid, k_simu, solo, interface, f_applied, fsteps, gait, ftps_Ids_deb, enable_hybrid_control=False, qmes=None, vmes=None, qmpc=None, vmpc=None): """Update the 3D desired position for feet in swing phase by using a 5-th order polynomial that lead them to the desired position on the ground (computed by the footstep planner) Args: qtsid (19x1 array): the position/orientation of the trunk and angular position of actuators vtsid (18x1 array): the linear/angular velocity of the trunk and angular velocity of actuators t (float): time elapsed since the start of the simulation k_simu (int): number of time steps since the start of the simulation solo (object): Pinocchio wrapper for the quadruped interface (Interface object): interface between the simulator and the MPC/InvDyn f_applied (12 array): desired contact forces for all feet (0s for feet in swing phase) fsteps (Xx13 array): contains the remaining number of steps of each phase of the gait (first column) and the [x, y, z]^T desired position of each foot for each phase of the gait (12 other columns). For feet currently touching the ground the desired position is where they currently are. enable_hybrid_control (bool): whether hybrid control is enabled or not qmes (19x1 array): the position/orientation of the trunk and angular position of actuators of the real robot (for hybrid control) vmes (18x1 array): the linear/angular velocity of the trunk and angular velocity of actuators of the real robot (for hybrid control) """ self.f_applied = f_applied # If hybrid control is turned on/off the CoM task needs to be turned on/off too """if self.enable_hybrid_control != enable_hybrid_control: if enable_hybrid_control: # Turn on CoM task self.invdyn.addMotionTask(self.comTask, self.w_com, 1, 0.0) else: # Turn off CoM task self.invdyn.removeTask(self.comTask, 0.0)""" # Update hybrid control parameters self.enable_hybrid_control = enable_hybrid_control if self.enable_hybrid_control: self.qmes = qmes self.vmes = vmes # Update state of TSID if k_simu == 0: # Some initialization during the first iteration self.qtsid = qtsid self.qtsid[:3] = np.zeros((3, 1)) # Discard x and y drift and height position self.qtsid[2, 0] = 0.235 - 0.01205385 self.feetGoal = 4*[None] self.sampleFeet = 4*[None] self.pos_contact = 4*[None] for i_foot in range(4): self.feetGoal[i_foot] = self.robot.framePosition( self.invdyn.data(), self.ID_feet[i_foot]) footTraj = tsid.TrajectorySE3Constant("foot_traj", self.feetGoal[i_foot]) self.sampleFeet[i_foot] = footTraj.computeNext() self.pos_contact[i_foot] = np.matrix([self.footsteps[0, i_foot], self.footsteps[1, i_foot], 0.0]) else: # Here is where we will merge the data from the state estimator and the internal state of TSID """# Encoders (position of joints) self.qtsid[7:] = qtsid[7:] # Gyroscopes (angular velocity of trunk) self.vtsid[3:6] = vtsid[3:6] # IMU estimation of orientation of the trunk self.qtsid[3:7] = qtsid[3:7]""" """self.qtsid = qtsid.copy() # self.qtsid[2] -= 0.015 # 0.01205385 self.vtsid = vtsid.copy() self.qtsid[2, 0] += mpc.q_noise[0] self.qtsid[3:7] = utils.getQuaternion(utils.quaternionToRPY( qtsid[3:7, 0]) + np.vstack((np.array([mpc.q_noise[1:]]).transpose(), 0.0))) self.vtsid[:6, 0] += mpc.v_noise""" # Update internal state of TSID for the current interation self.update_state(qtsid, vtsid) ##################### # FOOTSTEPS PLANNER # ##################### looping = int(self.n_periods*self.T_gait/dt) # Number of TSID iterations in one gait cycle k_loop = (k_simu - 0) % looping # Current number of iterations since the start of the current gait cycle # Update the desired position of footholds thanks to the footstep planner self.update_footsteps(interface, fsteps) ###################################### # UPDATE REFERENCE OF CONTACT FORCES # ###################################### # Update the contact force tracking tasks to follow the forces computed by the MPC self.update_ref_forces(interface) ################ # UPDATE TASKS # ################ # Enable/disable contact and 3D tracking tasks depending on the state of the feet (swing or stance phase) self.update_tasks(k_simu, k_loop, looping, interface, gait, ftps_Ids_deb) ############### # HQP PROBLEM # ############### # Solve the inverse dynamics problem with TSID self.solve_HQP_problem(self.t) # Time incrementation self.t += dt ########### # DISPLAY # ########### # Refresh Gepetto Viewer solo.display(self.qtsid) return self.tau
def __init__(self, conf, viewer=True): self.conf = conf vector = se3.StdVec_StdString() vector.extend(item for item in conf.path) self.robot = tsid.RobotWrapper(conf.urdf, vector, se3.JointModelFreeFlyer(), False) robot = self.robot self.model = robot.model() pin.loadReferenceConfigurations(self.model, conf.srdf, False) self.q0 = q = self.model.referenceConfigurations["half_sitting"] v = np.zeros(robot.nv) assert self.model.existFrame(conf.rf_frame_name) assert self.model.existFrame(conf.lf_frame_name) formulation = tsid.InverseDynamicsFormulationAccForce( "tsid", robot, False) formulation.computeProblemData(0.0, q, v) data = formulation.data() contact_Point = np.ones((3, 4)) * conf.lz contact_Point[0, :] = [-conf.lxn, -conf.lxn, conf.lxp, conf.lxp] contact_Point[1, :] = [-conf.lyn, conf.lyp, -conf.lyn, conf.lyp] contactRF = tsid.Contact6d("contact_rfoot", robot, conf.rf_frame_name, contact_Point, conf.contactNormal, conf.mu, conf.fMin, conf.fMax) contactRF.setKp(conf.kp_contact * np.ones(6)) contactRF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6)) self.RF = robot.model().getJointId(conf.rf_frame_name) H_rf_ref = robot.position(data, self.RF) # modify initial robot configuration so that foot is on the ground (z=0) q[2] -= H_rf_ref.translation[2] - conf.lz formulation.computeProblemData(0.0, q, v) data = formulation.data() H_rf_ref = robot.position(data, self.RF) contactRF.setReference(H_rf_ref) formulation.addRigidContact(contactRF, conf.w_forceRef) contactLF = tsid.Contact6d("contact_lfoot", robot, conf.lf_frame_name, contact_Point, conf.contactNormal, conf.mu, conf.fMin, conf.fMax) contactLF.setKp(conf.kp_contact * np.ones(6)) contactLF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6)) self.LF = robot.model().getJointId(conf.lf_frame_name) H_lf_ref = robot.position(data, self.LF) contactLF.setReference(H_lf_ref) formulation.addRigidContact(contactLF, conf.w_forceRef) comTask = tsid.TaskComEquality("task-com", robot) comTask.setKp(conf.kp_com * np.ones(3)) comTask.setKd(2.0 * np.sqrt(conf.kp_com) * np.ones(3)) formulation.addMotionTask(comTask, conf.w_com, 1, 0.0) postureTask = tsid.TaskJointPosture("task-posture", robot) postureTask.setKp(conf.kp_posture * np.ones(robot.nv - 6)) postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) * np.ones(robot.nv - 6)) formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0) self.leftFootTask = tsid.TaskSE3Equality("task-left-foot", self.robot, self.conf.lf_frame_name) self.leftFootTask.setKp(self.conf.kp_foot * np.ones(6)) self.leftFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6)) self.trajLF = tsid.TrajectorySE3Constant("traj-left-foot", H_lf_ref) formulation.addMotionTask(self.leftFootTask, self.conf.w_foot, 1, 0.0) self.rightFootTask = tsid.TaskSE3Equality("task-right-foot", self.robot, self.conf.rf_frame_name) self.rightFootTask.setKp(self.conf.kp_foot * np.ones(6)) self.rightFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6)) self.trajRF = tsid.TrajectorySE3Constant("traj-right-foot", H_rf_ref) formulation.addMotionTask(self.rightFootTask, self.conf.w_foot, 1, 0.0) self.tau_max = conf.tau_max_scaling * robot.model( ).effortLimit[-robot.na:] self.tau_min = -self.tau_max actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot) actuationBoundsTask.setBounds(self.tau_min, self.tau_max) if (conf.w_torque_bounds > 0.0): formulation.addActuationTask(actuationBoundsTask, conf.w_torque_bounds, 0, 0.0) jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, conf.dt) self.v_max = conf.v_max_scaling * robot.model( ).velocityLimit[-robot.na:] self.v_min = -self.v_max jointBoundsTask.setVelocityBounds(self.v_min, self.v_max) if (conf.w_joint_bounds > 0.0): formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, 0, 0.0) com_ref = robot.com(data) self.trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref) self.sample_com = self.trajCom.computeNext() q_ref = q[7:] self.trajPosture = tsid.TrajectoryEuclidianConstant( "traj_joint", q_ref) postureTask.setReference(self.trajPosture.computeNext()) self.sampleLF = self.trajLF.computeNext() self.sample_LF_pos = self.sampleLF.pos() self.sample_LF_vel = self.sampleLF.vel() self.sample_LF_acc = self.sampleLF.acc() self.sampleRF = self.trajRF.computeNext() self.sample_RF_pos = self.sampleRF.pos() self.sample_RF_vel = self.sampleRF.vel() self.sample_RF_acc = self.sampleRF.acc() self.solver = tsid.SolverHQuadProgFast("qp solver") self.solver.resize(formulation.nVar, formulation.nEq, formulation.nIn) self.comTask = comTask self.postureTask = postureTask self.contactRF = contactRF self.contactLF = contactLF self.actuationBoundsTask = actuationBoundsTask self.jointBoundsTask = jointBoundsTask self.formulation = formulation self.q = q self.v = v self.contact_LF_active = True self.contact_RF_active = True # for gepetto viewer if (viewer): self.robot_display = se3.RobotWrapper.BuildFromURDF( conf.urdf, [ conf.path, ], se3.JointModelFreeFlyer()) l = subprocess.getstatusoutput( "ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l") if int(l[1]) == 0: os.system('gepetto-gui &') time.sleep(1) gepetto.corbaserver.Client() self.robot_display.initViewer(loadModel=True) self.robot_display.displayCollisions(False) self.robot_display.displayVisuals(True) self.robot_display.display(q) self.gui = self.robot_display.viewer.gui # self.gui.setCameraTransform(0, conf.CAMERA_TRANSFORM) self.gui.addFloor('world/floor') self.gui.setLightingMode('world/floor', 'OFF')
def __init__(self): ######################################################################## # Definition of the Model and TSID problem # ######################################################################## # set the path path = "/opt/openrobots/share/example-robot-data/robots/solo_description/robots" urdf = path + "/solo12.urdf" srdf = "/opt/openrobots/share/example-robot-data/robots/solo_description/srdf/solo.srdf" vec = pin.StdVec_StdString() vec.extend(item for item in path) # get the solo12 wrapper self.solo12_wrapper = tsid.RobotWrapper(urdf, vec, pin.JointModelFreeFlyer(), False) self.solo12_model = self.solo12_wrapper.model() pin.loadReferenceConfigurations(self.solo12_model, srdf, False) # problem formulation self.formulation = tsid.InverseDynamicsFormulationAccForce( "tsid", self.solo12_wrapper, False) self.q0 = self.solo12_model.referenceConfigurations[ "straight_standing"] self.v0 = np.zeros(self.solo12_model.nv) self.formulation.computeProblemData(0.0, self.q0, self.v0) self.data = self.formulation.data() # get frame ID self.ID_FL = self.solo12_model.getFrameId("FL_FOOT") self.ID_FR = self.solo12_model.getFrameId("FR_FOOT") self.ID_HL = self.solo12_model.getFrameId("HL_FOOT") self.ID_HR = self.solo12_model.getFrameId("HR_FOOT") self.ID_BASE = self.solo12_model.getFrameId("base_link") ############ # COM TASK # ############ self.kp_com = 50 self.w_com = 3 self.comTask = tsid.TaskComEquality("task-com", self.solo12_wrapper) self.comTask.setKp(self.kp_com * np.ones(3).T) self.comTask.setKd(2 * np.sqrt(self.kp_com) * np.ones(3).T) self.formulation.addMotionTask(self.comTask, self.w_com, 1, 0.0) self.com_ref = self.solo12_wrapper.com(self.data) self.trajCom = tsid.TrajectoryEuclidianConstant( "traj-com", self.com_ref) ################# # SE3 BASE TASK # ################# self.kp_trunk = 50 self.w_trunk = 1 self.trunkTask = tsid.TaskSE3Equality("task-trunk", self.solo12_wrapper, 'base_link') mask = np.matrix([1.0, 1.0, 0.0, 1.0, 1.0, 1.0]).T self.trunkTask.setKp( np.matrix([ self.kp_trunk, self.kp_trunk, 0.0, self.kp_trunk, self.kp_trunk, self.kp_trunk ]).T) self.trunkTask.setKd( np.matrix([ 2.0 * np.sqrt(self.kp_trunk), 2.0 * np.sqrt(self.kp_trunk), 0.0, 2.0 * np.sqrt(self.kp_trunk), 2.0 * np.sqrt(self.kp_trunk), 2.0 * np.sqrt(self.kp_trunk) ]).T) self.trunkTask.useLocalFrame(False) self.trunkTask.setMask(mask) self.formulation.addMotionTask(self.trunkTask, self.w_trunk, 1, 0.0) self.trunk_ref = self.solo12_wrapper.framePosition( self.data, self.ID_BASE) self.trajTrunk = tsid.TrajectorySE3Constant("traj_base_link", self.trunk_ref) ################## # CONTACT FORCES # ################## self.contactNormal = np.array([0, 0, 1]) self.kp_contact = 30 self.mu = 0.5 self.contactFL = tsid.ContactPoint("contactFL", self.solo12_wrapper, "FL_FOOT", self.contactNormal, self.mu, 1, 1000) self.contactFL.setKp(self.kp_contact * np.ones(6).T) self.contactFL.setKd(2 * np.sqrt(self.kp_contact) * np.ones(6).T) self.fl_ref = self.solo12_wrapper.framePosition(self.data, self.ID_FL) self.contactFL.setReference(self.fl_ref) self.formulation.addRigidContact(self.contactFL, 10) self.contactFR = tsid.ContactPoint("contactFR", self.solo12_wrapper, "FR_FOOT", self.contactNormal, self.mu, 1, 1000) self.contactFR.setKp(self.kp_contact * np.ones(6).T) self.contactFR.setKd(2 * np.sqrt(self.kp_contact) * np.ones(6).T) self.fr_ref = self.solo12_wrapper.framePosition(self.data, self.ID_FR) self.contactFR.setReference(self.fr_ref) self.formulation.addRigidContact(self.contactFR, 10) self.contactHR = tsid.ContactPoint("contactHR", self.solo12_wrapper, "HR_FOOT", self.contactNormal, self.mu, 1, 1000) self.contactHR.setKp(self.kp_contact * np.ones(6).T) self.contactHR.setKd(2 * np.sqrt(self.kp_contact) * np.ones(6).T) self.hr_ref = self.solo12_wrapper.framePosition(self.data, self.ID_HR) self.contactHR.setReference(self.hr_ref) self.formulation.addRigidContact(self.contactHR, 10) self.contactHL = tsid.ContactPoint("contactHL", self.solo12_wrapper, "HL_FOOT", self.contactNormal, self.mu, 1, 1000) self.contactHL.setKp(self.kp_contact * np.ones(6).T) self.contactHL.setKd(2 * np.sqrt(self.kp_contact) * np.ones(6).T) self.hl_ref = self.solo12_wrapper.framePosition(self.data, self.ID_HL) self.contactHL.setReference(self.hl_ref) self.formulation.addRigidContact(self.contactHL, 10) ########## # SOLVER # ########## self.solver = tsid.SolverHQuadProgFast("qp solver") self.solver.resize(self.formulation.nVar, self.formulation.nEq, self.formulation.nIn)
def generateWholeBodyMotion(cs, fullBody=None, viewer=None): if not viewer: print "No viewer linked, cannot display end_effector trajectories." print "Start TSID ... " rp = RosPack() urdf = rp.get_path( cfg.Robot.packageName ) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf' if cfg.WB_VERBOSE: print "load robot : ", urdf #srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf' robot = tsid.RobotWrapper(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False) if cfg.WB_VERBOSE: print "robot loaded in tsid." # FIXME : tsid robotWrapper don't have all the required methods, only pinocchio have them pinRobot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False) q = cs.contact_phases[0].reference_configurations[0][:robot.nq].copy() v = np.matrix(np.zeros(robot.nv)).transpose() t = 0.0 # time # init states list with initial state (assume joint velocity is null for t=0) invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False) invdyn.computeProblemData(t, q, v) data = invdyn.data() if cfg.EFF_CHECK_COLLISION: # initialise object needed to check the motion from mlp.utils import check_path validator = check_path.PathChecker(fullBody, cs, len(q), cfg.WB_VERBOSE) if cfg.WB_VERBOSE: print "initialize tasks : " comTask = tsid.TaskComEquality("task-com", robot) comTask.setKp(cfg.kp_com * np.matrix(np.ones(3)).transpose()) comTask.setKd(2.0 * np.sqrt(cfg.kp_com) * np.matrix(np.ones(3)).transpose()) invdyn.addMotionTask(comTask, cfg.w_com, cfg.level_com, 0.0) com_ref = robot.com(invdyn.data()) trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref) amTask = tsid.TaskAMEquality("task-am", robot) amTask.setKp(cfg.kp_am * np.matrix([1., 1., 0.]).T) amTask.setKd(2.0 * np.sqrt(cfg.kp_am * np.matrix([1., 1., 0.]).T)) invdyn.addTask(amTask, cfg.w_am, cfg.level_am) trajAM = tsid.TrajectoryEuclidianConstant("traj_am", np.matrix(np.zeros(3)).T) postureTask = tsid.TaskJointPosture("task-joint-posture", robot) postureTask.setKp(cfg.kp_posture * cfg.gain_vector) postureTask.setKd(2.0 * np.sqrt(cfg.kp_posture * cfg.gain_vector)) postureTask.mask(cfg.masks_posture) invdyn.addMotionTask(postureTask, cfg.w_posture, cfg.level_posture, 0.0) q_ref = cfg.IK_REFERENCE_CONFIG trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref[7:]) orientationRootTask = tsid.TaskSE3Equality("task-orientation-root", robot, 'root_joint') mask = np.matrix(np.ones(6)).transpose() mask[0:3] = 0 mask[5] = cfg.YAW_ROT_GAIN orientationRootTask.setKp(cfg.kp_rootOrientation * mask) orientationRootTask.setKd(2.0 * np.sqrt(cfg.kp_rootOrientation * mask)) invdyn.addMotionTask(orientationRootTask, cfg.w_rootOrientation, cfg.level_rootOrientation, 0.0) root_ref = robot.position(data, robot.model().getJointId('root_joint')) trajRoot = tsid.TrajectorySE3Constant("traj-root", root_ref) usedEffectors = [] for eeName in cfg.Robot.dict_limb_joint.values(): if isContactEverActive(cs, eeName): usedEffectors.append(eeName) # init effector task objects : dic_effectors_tasks = createEffectorTasksDic(cs, robot) effectorTraj = tsid.TrajectorySE3Constant( "traj-effector", SE3.Identity() ) # trajectory doesn't matter as it's only used to get the correct struct and size # init empty dic to store effectors trajectories : dic_effectors_trajs = {} for eeName in usedEffectors: dic_effectors_trajs.update({eeName: None}) # add initial contacts : dic_contacts = {} for eeName in usedEffectors: if isContactActive(cs.contact_phases[0], eeName): contact = createContactForEffector(invdyn, robot, cs.contact_phases[0], eeName) dic_contacts.update({eeName: contact}) if cfg.PLOT: # init a dict storing all the reference trajectories used (for plotting) stored_effectors_ref = {} for eeName in dic_effectors_tasks: stored_effectors_ref.update({eeName: []}) solver = tsid.SolverHQuadProg("qp solver") solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn) # define nested function used in control loop def storeData(k_t, res, q, v, dv, invdyn, sol): # store current state res.q_t[:, k_t] = q res.dq_t[:, k_t] = v res.ddq_t[:, k_t] = dv res.tau_t[:, k_t] = invdyn.getActuatorForces( sol) # actuator forces, with external forces (contact forces) #store contact info (force and status) if cfg.IK_store_contact_forces: for eeName, contact in dic_contacts.iteritems(): if invdyn.checkContact(contact.name, sol): res.contact_forces[eeName][:, k_t] = invdyn.getContactForce( contact.name, sol) res.contact_normal_force[ eeName][:, k_t] = contact.getNormalForce( res.contact_forces[eeName][:, k_t]) res.contact_activity[eeName][:, k_t] = 1 # store centroidal info (real one and reference) : if cfg.IK_store_centroidal: pcom, vcom, acom = pinRobot.com(q, v, dv) res.c_t[:, k_t] = pcom res.dc_t[:, k_t] = vcom res.ddc_t[:, k_t] = acom res.L_t[:, k_t] = pinRobot.centroidalMomentum(q, v).angular #res.dL_t[:,k_t] = pinRobot.centroidalMomentumVariation(q,v,dv) # FIXME : in robot wrapper, use * instead of .dot() for np matrices pin.dccrba(pinRobot.model, pinRobot.data, q, v) res.dL_t[:, k_t] = Force( pinRobot.data.Ag.dot(dv) + pinRobot.data.dAg.dot(v)).angular # same for reference data : res.c_reference[:, k_t] = com_desired res.dc_reference[:, k_t] = vcom_desired res.ddc_reference[:, k_t] = acom_desired res.L_reference[:, k_t] = L_desired res.dL_reference[:, k_t] = dL_desired if cfg.IK_store_zmp: tau = pin.rnea( pinRobot.model, pinRobot.data, q, v, dv ) # tau without external forces, only used for the 6 first #res.tau_t[:6,k_t] = tau[:6] phi0 = pinRobot.data.oMi[1].act(Force(tau[:6])) res.wrench_t[:, k_t] = phi0.vector res.zmp_t[:, k_t] = shiftZMPtoFloorAltitude( cs, res.t_t[k_t], phi0, cfg.EXPORT_OPENHRP) # same but from the 'reference' values : Mcom = SE3.Identity() Mcom.translation = com_desired Fcom = Force.Zero() Fcom.linear = cfg.MASS * (acom_desired - cfg.GRAVITY) Fcom.angular = dL_desired F0 = Mcom.act(Fcom) res.wrench_reference[:, k_t] = F0.vector res.zmp_reference[:, k_t] = shiftZMPtoFloorAltitude( cs, res.t_t[k_t], F0, cfg.EXPORT_OPENHRP) if cfg.IK_store_effector: for eeName in usedEffectors: # real position (not reference) res.effector_trajectories[eeName][:, k_t] = SE3toVec( getCurrentEffectorPosition(robot, invdyn.data(), eeName)) res.d_effector_trajectories[eeName][:, k_t] = MotiontoVec( getCurrentEffectorVelocity(robot, invdyn.data(), eeName)) res.dd_effector_trajectories[eeName][:, k_t] = MotiontoVec( getCurrentEffectorAcceleration(robot, invdyn.data(), eeName)) return res def printIntermediate(v, dv, invdyn, sol): print "Time %.3f" % (t) for eeName, contact in dic_contacts.iteritems(): if invdyn.checkContact(contact.name, sol): f = invdyn.getContactForce(contact.name, sol) print "\tnormal force %s: %.1f" % (contact.name.ljust( 20, '.'), contact.getNormalForce(f)) print "\ttracking err %s: %.3f" % (comTask.name.ljust( 20, '.'), norm(comTask.position_error, 2)) for eeName, traj in dic_effectors_trajs.iteritems(): if traj: task = dic_effectors_tasks[eeName] error = task.position_error if cfg.Robot.cType == "_3_DOF": error = error[0:3] print "\ttracking err %s: %.3f" % (task.name.ljust( 20, '.'), norm(error, 2)) print "\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv)) def checkDiverge(res, v, dv): if norm(dv) > 1e6 or norm(v) > 1e6: print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" print "/!\ ABORT : controler unstable at t = " + str(t) + " /!\ " print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" raise ValueError("ABORT : controler unstable at t = " + str(t)) if math.isnan(norm(dv)) or math.isnan(norm(v)): print "!!!!!! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" print "/!\ ABORT : nan at t = " + str(t) + " /!\ " print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" raise ValueError("ABORT : controler unstable at t = " + str(t)) def stopHere(): if cfg.WB_ABORT_WHEN_INVALID: return res.resize(phase_interval[0]), pinRobot elif cfg.WB_RETURN_INVALID: return res.resize(k_t), pinRobot # time check dt = cfg.IK_dt if cfg.WB_VERBOSE: print "dt : ", dt res = Result(robot.nq, robot.nv, cfg.IK_dt, eeNames=usedEffectors, cs=cs) N = res.N last_display = 0 if cfg.WB_VERBOSE: print "tsid initialized, start control loop" #raw_input("Enter to start the motion (motion displayed as it's computed, may be slower than real-time)") time_start = time.time() # For each phases, create the necessary task and references trajectories : for pid in range(cs.size()): if cfg.WB_VERBOSE: print "## for phase : ", pid print "t = ", t phase = cs.contact_phases[pid] if pid < cs.size() - 1: phase_next = cs.contact_phases[pid + 1] else: phase_next = None if pid > 0: phase_prev = cs.contact_phases[pid - 1] else: phase_prev = None t_phase_begin = res.phases_intervals[pid][0] * dt t_phase_end = res.phases_intervals[pid][-1] * dt time_interval = [t_phase_begin, t_phase_end] if cfg.WB_VERBOSE: print "time_interval ", time_interval # generate com ref traj from phase : com_init = np.matrix(np.zeros((9, 1))) com_init[0:3, 0] = robot.com(invdyn.data()) com_traj = computeCOMRefFromPhase(phase, time_interval) am_traj = computeAMRefFromPhase(phase, time_interval) # add root's orientation ref from reference config : if cfg.USE_PLANNING_ROOT_ORIENTATION: if phase_next: root_traj = trajectories.TrajectorySE3LinearInterp( SE3FromConfig(phase.reference_configurations[0]), SE3FromConfig(phase_next.reference_configurations[0]), time_interval) else: root_traj = trajectories.TrajectorySE3LinearInterp( SE3FromConfig(phase.reference_configurations[0]), SE3FromConfig(phase.reference_configurations[0]), time_interval) else: # orientation such that the torso orientation is the mean between both feet yaw rotations: placement_init, placement_end = rootOrientationFromFeetPlacement( phase, phase_next) root_traj = trajectories.TrajectorySE3LinearInterp( placement_init, placement_end, time_interval) # add newly created contacts : for eeName in usedEffectors: if phase_prev and not isContactActive( phase_prev, eeName) and isContactActive(phase, eeName): invdyn.removeTask(dic_effectors_tasks[eeName].name, 0.0) # remove pin task for this contact dic_effectors_trajs.update( {eeName: None}) # delete reference trajectory for this task if cfg.WB_VERBOSE: print "remove se3 task : " + dic_effectors_tasks[ eeName].name contact = createContactForEffector(invdyn, robot, phase, eeName) dic_contacts.update({eeName: contact}) # add se3 tasks for end effector not in contact that will be in contact next phase: for eeName, task in dic_effectors_tasks.iteritems(): if phase_next and not isContactActive( phase, eeName) and isContactActive(phase_next, eeName): if cfg.WB_VERBOSE: print "add se3 task for " + eeName invdyn.addMotionTask(task, cfg.w_eff, cfg.level_eff, 0.0) #create reference trajectory for this task : placement_init = getCurrentEffectorPosition( robot, invdyn.data(), eeName ) #FIXME : adjust orientation in case of 3D contact ... if cfg.Robot.cType == "_3_DOF": placement_init.rotation = JointPlacementForEffector( phase, eeName).rotation placement_end = JointPlacementForEffector(phase_next, eeName) ref_traj = generateEndEffectorTraj(time_interval, placement_init, placement_end, 0) if cfg.WB_VERBOSE: print "t interval : ", time_interval print "placement Init : ", placement_init print "placement End : ", placement_end # display all the effector trajectories for this phase if viewer and cfg.DISPLAY_ALL_FEET_TRAJ: display_tools.displaySE3Traj( ref_traj, viewer.client.gui, viewer.sceneName, eeName + "_traj_" + str(pid), cfg.Robot.dict_limb_color_traj[eeName], time_interval, cfg.Robot.dict_offset[eeName]) viewer.client.gui.setVisibility( eeName + "_traj_" + str(pid), 'ALWAYS_ON_TOP') dic_effectors_trajs.update({eeName: ref_traj}) # start removing the contact that will be broken in the next phase : # (This tell the solver that it should start minimzing the contact force on this contact, and ideally get to 0 at the given time) for eeName, contact in dic_contacts.iteritems(): if phase_next and isContactActive( phase, eeName) and not isContactActive(phase_next, eeName): transition_time = t_phase_end - t - dt / 2. if cfg.WB_VERBOSE: print "\nTime %.3f Start breaking contact %s. transition time : %.3f\n" % ( t, contact.name, transition_time) invdyn.removeRigidContact(contact.name, transition_time) if cfg.WB_STOP_AT_EACH_PHASE: raw_input('start simulation') # save values at the beginning of the current phase q_begin = q.copy() v_begin = v.copy() phaseValid = False swingPhase = False # will be true if an effector move during this phase iter_for_phase = -1 # iterate until a valid motion for this phase is found (ie. collision free and which respect joint-limits) while not phaseValid: if iter_for_phase >= 0: # reset values to their value at the beginning of the current phase q = q_begin.copy() v = v_begin.copy() iter_for_phase += 1 if cfg.WB_VERBOSE: print "Start simulation for phase " + str( pid) + ", try number : " + str(iter_for_phase) # loop to generate states (q,v,a) for the current contact phase : if pid == cs.size() - 1: # last state phase_interval = res.phases_intervals[pid] else: phase_interval = res.phases_intervals[pid][:-1] for k_t in phase_interval: t = res.t_t[k_t] # set traj reference for current time : # com sampleCom = trajCom.computeNext() com_desired = com_traj(t)[0] vcom_desired = com_traj(t)[1] acom_desired = com_traj(t)[2] sampleCom.pos(com_desired) sampleCom.vel(vcom_desired) sampleCom.acc(acom_desired) comTask.setReference(sampleCom) # am sampleAM = trajAM.computeNext() L_desired = am_traj(t)[0] dL_desired = am_traj(t)[1] sampleAM.pos(L_desired) sampleAM.vel(dL_desired) amTask.setReference(sampleAM) # posture samplePosture = trajPosture.computeNext() #print "postural task ref : ",samplePosture.pos() postureTask.setReference(samplePosture) # root orientation : sampleRoot = trajRoot.computeNext() sampleRoot.pos(SE3toVec(root_traj(t)[0])) sampleRoot.vel(MotiontoVec(root_traj(t)[1])) orientationRootTask.setReference(sampleRoot) quat_waist = Quaternion(root_traj(t)[0].rotation) res.waist_orientation_reference[:, k_t] = np.matrix( [quat_waist.x, quat_waist.y, quat_waist.z, quat_waist.w]).T if cfg.WB_VERBOSE == 2: print "### references given : ###" print "com pos : ", sampleCom.pos() print "com vel : ", sampleCom.vel() print "com acc : ", sampleCom.acc() print "AM pos : ", sampleAM.pos() print "AM vel : ", sampleAM.vel() print "root pos : ", sampleRoot.pos() print "root vel : ", sampleRoot.vel() # end effector (if they exists) for eeName, traj in dic_effectors_trajs.iteritems(): if traj: swingPhase = True # there is an effector motion in this phase sampleEff = effectorTraj.computeNext() traj_t = traj(t) sampleEff.pos(SE3toVec(traj_t[0])) sampleEff.vel(MotiontoVec(traj_t[1])) dic_effectors_tasks[eeName].setReference(sampleEff) if cfg.WB_VERBOSE == 2: print "effector " + str(eeName) + " pos : " + str( sampleEff.pos()) print "effector " + str(eeName) + " vel : " + str( sampleEff.vel()) if cfg.IK_store_effector: res.effector_references[eeName][:, k_t] = SE3toVec( traj_t[0]) res.d_effector_references[ eeName][:, k_t] = MotiontoVec(traj_t[1]) res.dd_effector_references[ eeName][:, k_t] = MotiontoVec(traj_t[2]) elif cfg.IK_store_effector: if k_t == 0: res.effector_references[eeName][:, k_t] = SE3toVec( getCurrentEffectorPosition( robot, invdyn.data(), eeName)) else: res.effector_references[ eeName][:, k_t] = res.effector_references[ eeName][:, k_t - 1] res.d_effector_references[eeName][:, k_t] = np.matrix( np.zeros(6)).T res.dd_effector_references[eeName][:, k_t] = np.matrix( np.zeros(6)).T # solve HQP for the current time HQPData = invdyn.computeProblemData(t, q, v) if cfg.WB_VERBOSE and t < phase.time_trajectory[0] + dt: print "final data for phase ", pid HQPData.print_all() sol = solver.solve(HQPData) dv = invdyn.getAccelerations(sol) res = storeData(k_t, res, q, v, dv, invdyn, sol) # update state v_mean = v + 0.5 * dt * dv v += dt * dv q = pin.integrate(robot.model(), q, dt * v_mean) if cfg.WB_VERBOSE == 2: print "v = ", v print "dv = ", dv if cfg.WB_VERBOSE and int(t / dt) % cfg.IK_PRINT_N == 0: printIntermediate(v, dv, invdyn, sol) try: checkDiverge(res, v, dv) except ValueError: return stopHere() # end while t \in phase_t (loop for the current contact phase) if swingPhase and cfg.EFF_CHECK_COLLISION: phaseValid, t_invalid = validator.check_motion( res.q_t[:, phase_interval[0]:k_t]) #if iter_for_phase < 3 :# FIXME : debug only, force limb-rrt # phaseValid = False if not phaseValid: print "Phase " + str(pid) + " not valid at t = " + str( t_invalid) if t_invalid <= cfg.EFF_T_PREDEF or t_invalid >= ( (t_phase_end - t_phase_begin) - cfg.EFF_T_PREDEF): print "Motion is invalid during predefined phases, cannot change this." return stopHere() if effectorCanRetry(): print "Try new end effector trajectory." try: for eeName, oldTraj in dic_effectors_trajs.iteritems( ): if oldTraj: # update the traj in the map placement_init = JointPlacementForEffector( phase_prev, eeName) placement_end = JointPlacementForEffector( phase_next, eeName) ref_traj = generateEndEffectorTraj( time_interval, placement_init, placement_end, iter_for_phase + 1, res.q_t[:, phase_interval[0]:k_t], phase_prev, phase, phase_next, fullBody, eeName, viewer) dic_effectors_trajs.update( {eeName: ref_traj}) if viewer and cfg.DISPLAY_ALL_FEET_TRAJ: display_tools.displaySE3Traj( ref_traj, viewer.client.gui, viewer.sceneName, eeName + "_traj_" + str(pid), cfg. Robot.dict_limb_color_traj[eeName], time_interval, cfg.Robot.dict_offset[eeName]) viewer.client.gui.setVisibility( eeName + "_traj_" + str(pid), 'ALWAYS_ON_TOP') except ValueError, e: print "ERROR in generateEndEffectorTraj :" print e.message return stopHere() else: print "End effector method choosen do not allow retries, abort here." return stopHere() else: # no effector motions, phase always valid (or bypass the check) phaseValid = True if cfg.WB_VERBOSE: print "Phase " + str(pid) + " valid." if phaseValid: # display all the effector trajectories for this phase if viewer and cfg.DISPLAY_FEET_TRAJ and not cfg.DISPLAY_ALL_FEET_TRAJ: for eeName, ref_traj in dic_effectors_trajs.iteritems(): if ref_traj: display_tools.displaySE3Traj( ref_traj, viewer.client.gui, viewer.sceneName, eeName + "_traj_" + str(pid), cfg.Robot.dict_limb_color_traj[eeName], time_interval, cfg.Robot.dict_offset[eeName]) viewer.client.gui.setVisibility( eeName + "_traj_" + str(pid), 'ALWAYS_ON_TOP') if cfg.PLOT: # add current ref_traj to the list for plotting stored_effectors_ref[eeName] += [ref_traj]
def __init__(self, q0, omega, t): self.omega = omega self.qdes = q0.copy() self.vdes = np.zeros((8,1)) self.ades = np.zeros((8,1)) self.error = False kp_foot = 10.0 w_foot = 10.0 ######################################################################## # Definition of the Model and TSID problem # ######################################################################## ## Set the paths where the urdf and srdf file of the robot are registered modelPath = "/opt/openrobots/lib/python3.5/site-packages/../../../share/example-robot-data/robots" urdf = modelPath + "/solo_description/robots/solo.urdf" srdf = modelPath + "/solo_description/srdf/solo.srdf" vector = pin.StdVec_StdString() vector.extend(item for item in modelPath) ## Create the robot wrapper from the urdf model (without the free flyer) self.robot = tsid.RobotWrapper(urdf, vector, False) self.model = self.robot.model() ## Creation of the Invverse Dynamics HQP problem using the robot ## accelerations (base + joints) and the contact forces self.invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", self.robot, False) # Compute the problem data with a solver based on EiQuadProg self.invdyn.computeProblemData(t, self.qdes, self.vdes) # Get the initial data self.data = self.invdyn.data() # Task definition self.FRfootTask = tsid.TaskSE3Equality("FR-foot-placement", self.robot, 'FR_FOOT') self.FRfootTask.setKp(kp_foot * matlib.ones(6).T) self.FRfootTask.setKd(2.0 * np.sqrt(kp_foot) * matlib.ones(6).T) self.FRfootTask.setMask(np.matrix([[1,0,1, 0,0,0]]).T) #set a mask allowing only the transation upon x and z-axis self.FRfootTask.useLocalFrame(False) # Add the task to the HQP with weight = w_foot, priority level = 0 (as real constraint) and a transition duration = 0.0 self.invdyn.addMotionTask(self.FRfootTask, w_foot, 1, 0.0) ## TSID Trajectory pin.forwardKinematics(self.model, self.data, self.qdes) pin.updateFramePlacements(self.model, self.data) self.FR_foot_ref = self.robot.framePosition(self.data, self.model.getFrameId('FR_FOOT')) FRgoalx = self.FR_foot_ref.translation[0,0] + 0.1 FRgoalz = self.FR_foot_ref.translation[2,0] + 0.1 self.FR_foot_goal = self.FR_foot_ref.copy() self.FR_foot_goal.translation = np.matrix([FRgoalx, self.FR_foot_ref.translation[1,0], FRgoalz]).T self.trajFRfoot = tsid.TrajectorySE3Constant("traj_FR_foot", self.FR_foot_goal) # Set the trajectory as reference for the foot positionning task self.sampleFoot = self.trajFRfoot.computeNext() self.FRfootTask.setReference(self.sampleFoot) ## Initialization of the solver # Use EiquadprogFast solver self.solver = tsid.SolverHQuadProgFast("qp solver") # Resize the solver to fit the number of variables, equality and inequality constraints self.solver.resize(self.invdyn.nVar, self.invdyn.nEq, self.invdyn.nIn)
q[2] += 0.84 task_se3 = tsid.TaskSE3Equality("task-se3", robot, "RWristPitch") na = 6 Kp = 100 * np.matrix(np.ones(na)).transpose() Kd = 20.0 * np.matrix(np.ones(na)).transpose() task_se3.setKp(Kp) task_se3.setKd(Kd) assert np.linalg.norm(Kp - task_se3.Kp, 2) < tol assert np.linalg.norm(Kd - task_se3.Kd, 2) < tol M_ref = se3.SE3.Random() traj = tsid.TrajectorySE3Constant("traj_se3", M_ref) sample = tsid.TrajectorySample(0) t = 0.0 max_it = 1000 error_past = 1e100 for i in range(0, max_it): robot.computeAllTerms(data, q, v) sample = traj.computeNext() task_se3.setReference(sample) const = task_se3.compute(t, q, v, data) Jpinv = np.linalg.pinv(const.matrix, 1e-5) dv = Jpinv * const.vector
samplePosture = trajPosture.computeNext() postureTask.setReference(samplePosture) # FOOT Task pin.forwardKinematics(model, data, qdes) HL_foot_ref = robot.framePosition(data, model.getFrameId('HL_FOOT')) tHL = HL_foot_ref.translation goals = {"HL": -0.15, "HR": 0.75, "FL": 0.80, "FR": 0.75} HL_foot_goal = HL_foot_ref.copy() HL_foot_goal.translation = np.matrix([tHL[0, 0], tHL[1, 0], goals["HL"]]).T trajHLfoot = tsid.TrajectorySE3Constant("traj_HL_foot", HL_foot_goal) sampleHLfoot = trajHLfoot.computeNext() HLfootTask.setReference(sampleHLfoot) ## Initialisation of the solver # Use EiquadprogFast solver solver = tsid.SolverHQuadProgFast("qp solver") # Resize the solver to fit the number of variables, equality and inequality constraints solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn) ## Launch the simulation for i in range(N_SIMULATION):
def __init__(self, conf, dt, urdf, modelPath, srdf): self.conf = conf self.dt = dt vector = pin.StdVec_StdString() vector.extend(item for item in modelPath) self.robot = tsid.RobotWrapper(urdf, vector, pin.JointModelFreeFlyer(), False) robot = self.robot self.model = robot.model() pin.loadReferenceConfigurations(self.model, srdf, False) try: self.q0 = conf.q0 q = np.copy(conf.q0) except: self.q0 = self.model.referenceConfigurations["half_sitting"] q = np.copy(self.q0) self.v0 = v = np.zeros(robot.nv) assert self.model.existFrame(conf.rf_frame_name) assert self.model.existFrame(conf.lf_frame_name) formulation = tsid.InverseDynamicsFormulationAccForce( "tsid", robot, False) formulation.computeProblemData(0.0, q, v) data = formulation.data() contact_Point = np.ones((3, 4)) * conf.lz contact_Point[0, :] = [-conf.lxn, -conf.lxn, conf.lxp, conf.lxp] contact_Point[1, :] = [-conf.lyn, conf.lyp, -conf.lyn, conf.lyp] contactRF = tsid.Contact6d("contact_rfoot", robot, conf.rf_frame_name, contact_Point, conf.contact_normal, conf.mu, conf.fMin, conf.fMax) contactRF.setKp(conf.kp_contact * np.ones(6)) contactRF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6)) self.RF = robot.model().getFrameId(conf.rf_frame_name) H_rf_ref = robot.framePosition(formulation.data(), self.RF) # print('H RF\n', H_rf_ref.translation) # modify initial robot configuration so that foot is on the ground (z=0) q[2] -= H_rf_ref.translation[2] + conf.lz formulation.computeProblemData(0.0, q, v) data = formulation.data() H_rf_ref = robot.framePosition(data, self.RF) # print('H RF\n', H_rf_ref) contactRF.setReference(H_rf_ref) if (conf.w_contact >= 0.0): formulation.addRigidContact(contactRF, conf.w_forceRef, conf.w_contact, 1) else: formulation.addRigidContact(contactRF, conf.w_forceRef) contactLF = tsid.Contact6d("contact_lfoot", robot, conf.lf_frame_name, contact_Point, conf.contact_normal, conf.mu, conf.fMin, conf.fMax) contactLF.setKp(conf.kp_contact * np.ones(6)) contactLF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6)) self.LF = robot.model().getFrameId(conf.lf_frame_name) H_lf_ref = robot.framePosition(formulation.data(), self.LF) # print('H LF\n', H_lf_ref) contactLF.setReference(H_lf_ref) if (conf.w_contact >= 0.0): formulation.addRigidContact(contactLF, conf.w_forceRef, conf.w_contact, 1) else: formulation.addRigidContact(contactLF, conf.w_forceRef) comTask = tsid.TaskComEquality("task-com", robot) comTask.setKp(conf.kp_com * np.ones(3)) comTask.setKd(2.0 * np.sqrt(conf.kp_com) * np.ones(3)) if (conf.w_com > 0): formulation.addMotionTask(comTask, conf.w_com, 1, 0.0) postureTask = tsid.TaskJointPosture("task-posture", robot) try: postureTask.setKp(conf.kp_posture) postureTask.setKd(2.0 * np.sqrt(conf.kp_posture)) except: postureTask.setKp(conf.kp_posture * np.ones(robot.nv - 6)) postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) * np.ones(robot.nv - 6)) if (conf.w_posture > 0): formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0) self.leftFootTask = tsid.TaskSE3Equality("task-left-foot", self.robot, self.conf.lf_frame_name) self.leftFootTask.setKp(self.conf.kp_foot * np.ones(6)) self.leftFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6)) self.trajLF = tsid.TrajectorySE3Constant("traj-left-foot", H_lf_ref) if (conf.w_foot > 0): formulation.addMotionTask(self.leftFootTask, self.conf.w_foot, 1, 0.0) # self.rightFootTask = tsid.TaskSE3Equality("task-right-foot", self.robot, self.conf.rf_frame_name) self.rightFootTask.setKp(self.conf.kp_foot * np.ones(6)) self.rightFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6)) self.trajRF = tsid.TrajectorySE3Constant("traj-right-foot", H_rf_ref) if (conf.w_foot > 0): formulation.addMotionTask(self.rightFootTask, self.conf.w_foot, 1, 0.0) self.waistTask = tsid.TaskSE3Equality("task-waist", self.robot, self.conf.waist_frame_name) self.waistTask.setKp(self.conf.kp_waist * np.ones(6)) self.waistTask.setKd(2.0 * np.sqrt(self.conf.kp_waist) * np.ones(6)) self.waistTask.setMask(np.array([0, 0, 0, 1, 1, 1.])) waistID = robot.model().getFrameId(conf.waist_frame_name) H_waist_ref = robot.framePosition(formulation.data(), waistID) self.trajWaist = tsid.TrajectorySE3Constant("traj-waist", H_waist_ref) if (conf.w_waist > 0): formulation.addMotionTask(self.waistTask, self.conf.w_waist, 1, 0.0) self.tau_max = conf.tau_max_scaling * robot.model( ).effortLimit[-robot.na:] self.tau_min = -self.tau_max actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot) actuationBoundsTask.setBounds(self.tau_min, self.tau_max) if (conf.w_torque_bounds > 0.0): formulation.addActuationTask(actuationBoundsTask, conf.w_torque_bounds, 0, 0.0) jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, dt) self.v_max = conf.v_max_scaling * robot.model( ).velocityLimit[-robot.na:] self.v_min = -self.v_max jointBoundsTask.setVelocityBounds(self.v_min, self.v_max) if (conf.w_joint_bounds > 0.0): formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, 0, 0.0) com_ref = robot.com(formulation.data()) self.trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref) self.sample_com = self.trajCom.computeNext() q_ref = q[7:] self.trajPosture = tsid.TrajectoryEuclidianConstant( "traj_joint", q_ref) postureTask.setReference(self.trajPosture.computeNext()) self.sampleLF = self.trajLF.computeNext() self.sample_LF_pos = self.sampleLF.pos() self.sample_LF_vel = self.sampleLF.vel() self.sample_LF_acc = self.sampleLF.acc() self.sampleRF = self.trajRF.computeNext() self.sample_RF_pos = self.sampleRF.pos() self.sample_RF_vel = self.sampleRF.vel() self.sample_RF_acc = self.sampleRF.acc() self.solver = tsid.SolverHQuadProgFast("qp solver") self.solver.resize(formulation.nVar, formulation.nEq, formulation.nIn) self.comTask = comTask self.postureTask = postureTask self.contactRF = contactRF self.contactLF = contactLF self.actuationBoundsTask = actuationBoundsTask self.jointBoundsTask = jointBoundsTask self.formulation = formulation self.q = q self.v = v self.q0 = np.copy(self.q) self.contact_LF_active = True self.contact_RF_active = True # self.reset() data = np.load(conf.DATA_FILE_TSID) # assume dt>dt_ref r = int(dt / conf.dt_ref) self.N = int(data['com'].shape[1] / r) self.contact_phase = data['contact_phase'][::r] self.com_pos_ref = np.asarray(data['com'])[:, ::r] self.com_vel_ref = np.asarray(data['dcom'])[:, ::r] self.com_acc_ref = np.asarray(data['ddcom'])[:, ::r] self.x_RF_ref = np.asarray(data['x_RF'])[:, ::r] self.dx_RF_ref = np.asarray(data['dx_RF'])[:, ::r] self.ddx_RF_ref = np.asarray(data['ddx_RF'])[:, ::r] self.x_LF_ref = np.asarray(data['x_LF'])[:, ::r] self.dx_LF_ref = np.asarray(data['dx_LF'])[:, ::r] self.ddx_LF_ref = np.asarray(data['ddx_LF'])[:, ::r] self.cop_ref = np.asarray(data['cop'])[:, ::r] x_rf = self.get_placement_RF().translation offset = x_rf - self.x_RF_ref[:, 0] # print("offset", offset) for i in range(self.N): self.com_pos_ref[:, i] += offset self.x_RF_ref[:, i] += offset self.x_LF_ref[:, i] += offset
def callback_torques(): global init, sol, t, v_prev, q, qdes, vdes, trajCom, trajFLfoot, trajFRfoot, trajHLfoot, trajHRfoot, comTask, FLfootTask, FRfootTask, HLfootTask, HRfootTask jointStates = p.getJointStates(robotId, revoluteJointIndices) # State of all joints baseState = p.getBasePositionAndOrientation(robotId) baseVel = p.getBaseVelocity(robotId) # Info about contact points with the ground contactPoints_FL = p.getContactPoints(robotId, planeId, linkIndexA=2) # Front left foot contactPoints_FR = p.getContactPoints(robotId, planeId, linkIndexA=5) # Front right foot contactPoints_HL = p.getContactPoints(robotId, planeId, linkIndexA=8) # Hind left foot contactPoints_HR = p.getContactPoints(robotId, planeId, linkIndexA=11) # Hind right foot # Sort contacts points to get only one contact per foot contactPoints = [] contactPoints.append(getContactPoint(contactPoints_FL)) contactPoints.append(getContactPoint(contactPoints_FR)) contactPoints.append(getContactPoint(contactPoints_HL)) contactPoints.append(getContactPoint(contactPoints_HR)) # Joint vector for Pinocchio q = np.vstack( (np.array([baseState[0]]).transpose(), np.array([baseState[1] ]).transpose(), np.array([[ jointStates[i_joint][0] for i_joint in range(len(jointStates)) ]]).transpose())) v = np.vstack( (np.array([baseVel[0]]).transpose(), np.array([baseVel[1] ]).transpose(), np.array([[ jointStates[i_joint][1] for i_joint in range(len(jointStates)) ]]).transpose())) v_dot = (v - v_prev) / dt v_prev = v.copy() #################################################################### pin.forwardKinematics(model, data, q) HL_foot_ref = robot.framePosition(data, model.getFrameId('HL_FOOT')) HR_foot_ref = robot.framePosition(data, model.getFrameId('HR_FOOT')) FL_foot_ref = robot.framePosition(data, model.getFrameId('FL_FOOT')) FR_foot_ref = robot.framePosition(data, model.getFrameId('FR_FOOT')) base = robot.framePosition(data, model.getFrameId('base_link')) tHL = HL_foot_ref.translation tHR = HR_foot_ref.translation tFL = FL_foot_ref.translation tFR = FR_foot_ref.translation tbase = base.translation ztFR = tFR[2, 0] zGoal = tbase[ 2, 0] - 0.235 # altitude objective : 0 from basis reference frame print(tbase[2, 0]) FR_foot_goal = FR_foot_ref.copy() FR_foot_goal.translation = np.matrix([tFR[0, 0], tFR[1, 0], zGoal]).T FL_foot_goal = FL_foot_ref.copy() FL_foot_goal.translation = np.matrix([tFL[0, 0], tFL[1, 0], zGoal]).T HR_foot_goal = HR_foot_ref.copy() HR_foot_goal.translation = np.matrix([tHR[0, 0], tHR[1, 0], zGoal]).T HL_foot_goal = HL_foot_ref.copy() HL_foot_goal.translation = np.matrix([tHL[0, 0], tHL[1, 0], zGoal]).T trajHLfoot = tsid.TrajectorySE3Constant("traj_HL_foot", HL_foot_goal) trajHRfoot = tsid.TrajectorySE3Constant("traj_HR_foot", HR_foot_goal) trajFLfoot = tsid.TrajectorySE3Constant("traj_FL_foot", FL_foot_goal) trajFRfoot = tsid.TrajectorySE3Constant("traj_FR_foot", FR_foot_goal) samplePosture = trajPosture.computeNext() postureTask.setReference(samplePosture) sampleHLfoot = trajHLfoot.computeNext() HLfootTask.setReference(sampleHLfoot) sampleHRfoot = trajHRfoot.computeNext() HRfootTask.setReference(sampleHRfoot) sampleFLfoot = trajFLfoot.computeNext() FLfootTask.setReference(sampleFLfoot) sampleFRfoot = trajFRfoot.computeNext() FRfootTask.setReference(sampleFRfoot) HQPData = invdyn.computeProblemData(t, qdes, vdes) sol = solver.solve(HQPData) tau = invdyn.getActuatorForces(sol) dv = invdyn.getAccelerations(sol) v_mean = vdes + 0.5 * dt * dv vdes += dt * dv qdes = pin.integrate(model, qdes, dt * v_mean) t += dt robot_display.display(q) qlist.append(q) #################################################################### # PD Torque controller Kp_PD = 10. Kd_PD = 0.1 #Kd = 2 * np.sqrt(2*Kp) # formula to get a critical damping torques = Kp_PD * (qdes[7:] - q[7:]) + Kd_PD * (vdes[6:] - v[6:]) # Saturation to limit the maximal torque t_max = 5 torques = np.maximum(np.minimum(torques, t_max * np.ones((12, 1))), -t_max * np.ones((12, 1))) return torques