def df_dq(model,func,q,h=1e-9): """ Perform df/dq by num_diff. q is in the lie manifold. :params func: function to differentiate f : np.matrix -> np.matrix :params q: configuration value at which f is differentiated. type np.matrix :params h: eps :returns df/dq """ dq = zero(model.nv) f0 = func(q) res = zero([len(f0),model.nv]) for iq in range(model.nv): dq[iq] = h res[:,iq] = (func(pin.integrate(model,q,dq)) - f0)/h dq[iq] = 0 return res
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
def vq2q(self, vq): return pinocchio.integrate(rmodel, self.q0, vq)
import pinocchio model = pinocchio.buildSampleModelManipulator() data = model.createData() JOINT_ID = 6 xdes = np.matrix([ 0.5,-0.5,0.5]).T q = model.neutralConfiguration eps = 1e-4 IT_MAX = 1000 DT = 1e-1 for i in range(IT_MAX): pinocchio.forwardKinematics(model,data,q) x = data.oMi[JOINT_ID].translation R = data.oMi[JOINT_ID].rotation err = R.T*(x-xdes) if np.linalg.norm(err) < eps: print("Convergence achieved!") break J = pinocchio.jointJacobian(model,data,q,JOINT_ID,pinocchio.ReferenceFrame.LOCAL,True)[:3,:] v = - np.linalg.pinv(J)*err q = pinocchio.integrate(model,q,v*DT) if not i % 10: print('error = %s' % (x-xdes).T) else: print("\nWarning: the iterative algorithm has not reached convergence to the desired precision") print('\nresult: %s' % q.flatten().tolist()) print('\nfinal error: %s' % (x-xdes).T)
dcrba = DCRBA(robot) dcrba.pre(q) Mp = dcrba() # --- Validate dM/dq by finite diff dM = np.zeros([robot.model.nv,]*3) eps = 1e-6 dq = zero(robot.model.nv) for diff in range(robot.model.nv): dM[:,:,diff] = -pin.crba(robot.model,robot.data,q) dq *=0; dq[diff] = eps qdq = pin.integrate(robot.model,q,dq) dM[:,:,diff] += pin.crba(robot.model,robot.data,qdq) dM /= eps print('Check dCRBA = \t\t\t', max([ norm(Mp[:,:,diff]-dM[:,:,diff]) for diff in range(robot.model.nv) ])) # --- vRNEA --- # --- vRNEA --- # --- vRNEA --- vrnea = VRNEA(robot) Q = vrnea(q)
def update_configuration(self, delta_q): self.q = pinocchio.integrate(self.model, self.q, delta_q)
herr = [] # Log the value of the error between gaze and ball. # Loop on an inverse kinematics for 200 iterations. for i in range(200): # Integrate over 2 second of robot life pio.framesForwardKinematics(robot.model, robot.data, q) # Compute frame placements oMgaze = robot.data.oMf[ IDX_GAZE] # Placement from world frame o to frame f oMgaze oRgaze = oMgaze.rotation # Rotation from world axes to gaze axes oRgaze gaze_Jgaze = pio.computeFrameJacobian( robot.model, robot.data, q, IDX_GAZE) # 6D jacobian in local frame o_Jgaze3 = oRgaze * gaze_Jgaze[:3, :] # 3D jacobian in world frame o_GazeBall = oMgaze.translation - ball # vector from gaze to ball, in world frame vq = -pinv(o_Jgaze3) * o_GazeBall q = pio.integrate(robot.model, q, vq * DT) robot.display(q) time.sleep(1e-3) herr.append(o_GazeBall) q = q0.copy() herr = [] # Log the value of the error between tool and goal. herr2 = [] # Log the value of the error between gaze and ball. # Loop on an inverse kinematics for 200 iterations. for i in range(200): # Integrate over 2 second of robot life pio.framesForwardKinematics(robot.model, robot.data, q) # Compute frame placements oMgaze = robot.data.oMf[ IDX_GAZE] # Placement from world frame o to frame f oMgaze oRgaze = oMgaze.rotation # Rotation from world axes to gaze axes oRgaze
sol.status) break tau[:, i] = formulation.getActuatorForces(sol) dv[:, i] = formulation.getAccelerations(sol) dv_des[:, i] = postureTask.getDesiredAcceleration if i % conf.PRINT_N == 0: print("Time %.3f" % (t)) print("\ttracking err %s: %.3f" % (postureTask.name.ljust( 20, '.'), norm(postureTask.position_error, 2))) # numerical integration v_mean = v[:, i] + 0.5 * dt * dv[:, i] v[:, i + 1] = v[:, i] + dt * dv[:, i] q[:, i + 1] = se3.integrate(model, q[:, i], dt * v_mean) t += conf.dt if i % conf.DISPLAY_N == 0: robot_display.display(q[:, i]) time_spent = time.time() - time_start if (time_spent < conf.dt): time.sleep(conf.dt - time_spent) # PLOT STUFF time = np.arange(0.0, N * conf.dt, conf.dt) if (PLOT_JOINT_POS): (f, ax) = plut.create_empty_figure(int(robot.nv / 2), 2) ax = ax.reshape(robot.nv) for i in range(robot.nv):
vq = rand(robot.model.nv) aq = rand(robot.model.nv) tau = rand(robot.model.nv) # Motor torque # """ M(q) ddq + C(q, dq) + G(q) = Tau b = C(q, dq) + G(q) (bias term as in Featherstone) """ # compute dynamic drift -- Coriolis, centrifugal, gravity b = pin.rnea(robot.model, robot.data, q, vq, np.zeros_like(aq)) # compute mass matrix M M = pin.crba(robot.model, robot.data, q) ddq = np.linalg.solve(M, (tau - b)) # valid only when no collision dt, dq = 0.01, 0 dq += ddq * dt q = pin.integrate(robot.model, q, dq * dt) # # M = pin.crba(robot, robot.data, q) # # # d/dq M(q) # dcrba = DCRBA(robot) # dcrba.pre(q) # Mp = dcrba() # # # # d/dvq RNEA(q,vq) = C(q,vq) # coriolis = Coriolis(robot) # C = coriolis(q,vq) # # # # # d/dq RNEA(q,vq,aq) # drnea = DRNEA(robot) # R = drnea(q,vq,aq) # print('hi')
def integrate(self,q,vq): ''' Given a configuration q and displacement dq = vq*dt, returns the integration of this displacement, i.e. something that looks like q+dq. ''' return pinocchio.integrate(self.model,q,vq)
import numpy as np import scipy as sp import time PKG = '/opt/openrobots/share' URDF = join(PKG, 'ur_description/urdf/ur5_gripper.urdf') robot = RobotWrapper(URDF, [PKG]) robot.initDisplay(loadModel=True) #Initial Set up & Variables q = rand(robot.nq) vq = zero(robot.nv) aq = zero(robot.nv) se3.forwardKinematics(robot.model, robot.data, q) robot.display(q) input_specified = None dt = 0.001 while 1: if input_specified: pass else: tau = np.full((1, robot.nv), 10).T #tau = zero(robot.nv) aq = se3.aba(robot.model, robot.data, q, vq, tau) vq += aq * dt q = se3.integrate(robot.model, q, vq * dt) robot.display(q)
Jpinv = np.zeros((robot.nv, 3)) error_past = 1e100 robot.v = np.zeros(robot.nv) for i in range(0, max_it): se3.computeAllTerms(model, data, robot.q, robot.v) sample = traj.computeNext() taskCOM.setReference(sample) const = taskCOM.compute(t, robot.q, robot.v) Jpinv = np.linalg.pinv(const.getMatrix(), 1e-5) dv = np.dot(Jpinv, const.getVector().T) assert np.linalg.norm(np.dot(Jpinv, const.getMatrix()), 2) - 1.0 < tol robot.v += dt * dv robot.q = se3.integrate(model, robot.q, dt * robot.v) t += dt error = np.linalg.norm(taskCOM.position_error(), 2) assert error - error_past < 1e-4 error_past = error if error < 1e-8: print("Success Convergence") break if i % 100 == 0: print("Time :", t, "COM pos error :", error, "COM vel error :", np.linalg.norm(taskCOM.velocity_error(), 2)) print("") print("Test Task Joint Posture") print("")
sol.status) break tau[:, i] = formulation.getActuatorForces(sol) dv[:, i] = formulation.getAccelerations(sol) dv_des[:, i] = postureTask.getDesiredAcceleration if i % conf.PRINT_N == 0: print("Time %.3f" % (t)) print("\ttracking err %s: %.3f" % (postureTask.name.ljust( 20, '.'), norm(postureTask.position_error, 2))) # numerical integration v_mean = v[:, i] + 0.5 * dt * dv[:, i] v[:, i + 1] = v[:, i] + dt * dv[:, i] q[:, i + 1] = pin.integrate(model, q[:, i], dt * v_mean) t += conf.dt if i % conf.DISPLAY_N == 0: robot_display.display(q[:, i]) time_spent = time.time() - time_start if (time_spent < conf.dt): time.sleep(conf.dt - time_spent) # PLOT STUFF time = np.arange(0.0, N * conf.dt, conf.dt) if (PLOT_JOINT_POS): (f, ax) = plut.create_empty_figure(int(robot.nv / 2), 2) ax = ax.reshape(robot.nv) for i in range(robot.nv):
model.lowerPositionLimit.fill(-math.pi) model.upperPositionLimit.fill(+math.pi) if args.with_cart: model.lowerPositionLimit[0] = model.upperPositionLimit[0] = 0. data_sim = model.createData() t = 0. q = pin.randomConfiguration(model) v = np.zeros((model.nv)) tau_control = np.zeros((model.nv)) damping_value = 0.1 for k in range(N): tic = time.time() tau_control = -damping_value * v # small damping a = pin.aba(model, data_sim, q, v, tau_control) # Forward dynamics # Semi-explicit integration v += a * dt q = pin.integrate(model, q, v * dt) # Configuration integration viz.display(q) toc = time.time() ellapsed = toc - tic dt_sleep = max(0, dt - (ellapsed)) time.sleep(dt_sleep) t += dt
tau_old = tau if i % PRINT_N == 0: print "Time ", i if invdyn.checkContact(contactRF.name, sol): f = invdyn.getContactForce(contactRF.name, sol) print " ", contactRF.name, "force: ", contactRF.getNormalForce( f) if invdyn.checkContact(contactLF.name, sol): f = invdyn.getContactForce(contactLF.name, sol) print " ", contactLF.name, "force: ", contactLF.getNormalForce( f) print " ", comTask.name, " err:", norm(comTask.position_error, 2) print " ", "v: ", norm(v, 2), "dv: ", norm(dv) v += dt * dv q = se3.integrate(model, q, dt * v) t += dt assert norm(dv) < 1e6 assert norm(v) < 1e6 data_2 = model.createData() # robot.data() com = se3.centerOfMass(model, data_2, q) com_correction = com - comTask.position_error # validate that it works print "Final COM Position", com_correction.transpose() print "Desired COM Position", com_ref.transpose()
Jpinv = np.matrix(np.zeros((robot.nv, 3))) error_past = 1e100 v = np.matrix(np.zeros(robot.nv)).transpose() for i in range(0, max_it): robot.computeAllTerms(data, q, v) sample = traj.computeNext() taskCOM.setReference(sample) const = taskCOM.compute(t, q, v, data) Jpinv = np.linalg.pinv(const.matrix, 1e-5) dv = Jpinv * const.vector assert np.linalg.norm(Jpinv * const.matrix, 2) - 1.0 < tol v += dt * dv q = pin.integrate(model, q, dt * v) t += dt error = np.linalg.norm(taskCOM.position_error, 2) assert error - error_past < 1e-4 error_past = error if error < 1e-8: print("Success Convergence") break if i % 100 == 0: print("Time :", t, "COM pos error :", error, "COM vel error :", np.linalg.norm(taskCOM.velocity_error, 2)) print("") print("Test Task Joint Posture") print("")
def c_walking(q, qdot, dt, solo, t_simu): # unactuated, [x, y, z] position of the base + [x, y, z, w] orientation of the base (stored as a quaternion) # qu = q[:7] qa = q[7:] # actuated, [q1, q2, ..., q8] angular position of the 8 motors # [v_x, v_y, v_z] linear velocity of the base and [w_x, w_y, w_z] angular velocity of the base along x, y, z axes # of the world # qu_dot = qdot[:6] qa_dot = qdot[6:] # angular velocity of the 8 motors qa_ref = np.zeros((8, 1)) # target angular positions for the motors qa_dot_ref = np.zeros((8, 1)) # target angular velocities for the motors ############################################### # Insert code here to set qa_ref and qa_dot_ref from numpy.linalg import pinv # Method : Inverse Kinematics global q_ref, flag_q_ref, T, dx, dz if flag_q_ref: q_ref = solo.q0.copy() flag_q_ref = False # Initialization of the variables Kp = 100. # Convergence gain T = 0.2 # period of the foot trajectory xF0 = 0.19 # initial position of the front feet xH0 = -0.19 # initial position of the hind feet z0 = 0.0 # initial altitude of each foot dx = 0.03 # displacement amplitude by x dz = 0.06 # displacement amplitude by z # Get the frame index of each foot ID_FL = solo.model.getFrameId("FL_FOOT") ID_FR = solo.model.getFrameId("FR_FOOT") ID_HL = solo.model.getFrameId("HL_FOOT") ID_HR = solo.model.getFrameId("HR_FOOT") # function defining the feet's trajectory def ftraj(t, x0, z0): # arguments : time, initial position x and z global T, dx, dz x = [] z = [] if t >= T: t %= T x.append(x0 - dx * np.cos(2 * np.pi * t / T)) if t <= T / 2.: z.append(z0 + dz * np.sin(2 * np.pi * t / T)) else: z.append(0) return np.matrix([x, z]) # Compute/update all the joints and frames pin.forwardKinematics(solo.model, solo.data, q_ref) pin.updateFramePlacements(solo.model, solo.data) # Get the current height (on axis z) and the x-coordinate of the front left foot xz_FL = solo.data.oMf[ID_FL].translation[0::2] xz_FR = solo.data.oMf[ID_FR].translation[0::2] xz_HL = solo.data.oMf[ID_HL].translation[0::2] xz_HR = solo.data.oMf[ID_HR].translation[0::2] # Desired foot trajectory xzdes_FL = ftraj(t_simu, xF0, z0) xzdes_HR = ftraj(t_simu, xH0, z0) xzdes_FR = ftraj(t_simu + T / 2, xF0, z0) xzdes_HL = ftraj(t_simu + T / 2, xH0, z0) # Calculating the error err_FL = xz_FL - xzdes_FL err_FR = xz_FR - xzdes_FR err_HL = xz_HL - xzdes_HL err_HR = xz_HR - xzdes_HR # Computing the local Jacobian into the global frame oR_FL = solo.data.oMf[ID_FL].rotation oR_FR = solo.data.oMf[ID_FR].rotation oR_HL = solo.data.oMf[ID_HL].rotation oR_HR = solo.data.oMf[ID_HR].rotation # Getting the different Jacobians fJ_FL3 = pin.frameJacobian(solo.model, solo.data, q_ref, ID_FL)[:3, -8:] # Take only the translation terms oJ_FL3 = oR_FL * fJ_FL3 # Transformation from local frame to world frame oJ_FLxz = oJ_FL3[0::2, -8:] # Take the x and z components fJ_FR3 = pin.frameJacobian(solo.model, solo.data, q_ref, ID_FR)[:3, -8:] oJ_FR3 = oR_FR * fJ_FR3 oJ_FRxz = oJ_FR3[0::2, -8:] fJ_HL3 = pin.frameJacobian(solo.model, solo.data, q_ref, ID_HL)[:3, -8:] oJ_HL3 = oR_HL * fJ_HL3 oJ_HLxz = oJ_HL3[0::2, -8:] fJ_HR3 = pin.frameJacobian(solo.model, solo.data, q_ref, ID_HR)[:3, -8:] oJ_HR3 = oR_HR * fJ_HR3 oJ_HRxz = oJ_HR3[0::2, -8:] # Displacement error nu = np.vstack([err_FL, err_FR, err_HL, err_HR]) # Making a single x&z-rows Jacobian vector J = np.vstack([oJ_FLxz, oJ_FRxz, oJ_HLxz, oJ_HRxz]) # Computing the velocity qa_dot_ref = -Kp * pinv(J) * nu q_dot_ref = np.concatenate((np.zeros([6, 1]), qa_dot_ref)) # Computing the updated configuration q_ref = pin.integrate(solo.model, q_ref, q_dot_ref * dt) qa_ref = q_ref[7:] solo.display(q) # End of the control code ############################################### # Parameters for the PD controller Kp = 8. Kd = 0.2 torques_sat = 3 * np.ones((8, 1)) # torque saturation in N.m torques_ref = np.zeros((8, 1)) # feedforward torques # Call the PD controller torques = PD(qa_ref, qa_dot_ref, qa, qa_dot, dt, Kp, Kd, torques_sat, torques_ref) # torques must be a numpy array of shape (8, 1) containing the torques applied to the 8 motors return torques
solo = robots_loader.loadSolo() solo.initDisplay(loadModel=True) q = solo.q0 solo.display(q) vq = np.zeros([14, 1]) dt = 1e-2 # loop over space navigator events while not stop: # wait for next event event = sp.wait() # if event signals the release of the first button if type(event) is sp.ButtonEvent \ and event.button == 0 and event.pressed == 0: # set exit condition stop = True # matching the mouse signals with the position of Solo's basis if type(event) is sp.MotionEvent: vq[3] = -event.rx / 100.0 vq[5] = event.ry / 100.0 #vq[4] = event.rz/100.0 q = pin.integrate(solo.model, q, vq * dt) solo.display(q) embed()
DT = 1e-1 damp = 1e-6 i = 0 while True: pinocchio.forwardKinematics(model, data, q) dMi = oMdes.actInv(data.oMi[JOINT_ID]) err = pinocchio.log(dMi).vector if norm(err) < eps: success = True break if i >= IT_MAX: success = False break J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID) v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err)) q = pinocchio.integrate(model, q, v * DT) if not i % 10: print('%d: error = %s' % (i, err.T)) i += 1 if success: print("Convergence achieved!") else: print( "\nWarning: the iterative algorithm has not reached convergence to the desired precision" ) print('\nresult: %s' % q.flatten().tolist()) print('\nfinal error: %s' % err.T)
def step(self, u, dt=None): if dt is None: dt = self.dt # (Forces are directly in the world frame, and aba wants them in the end effector frame) se3.computeAllTerms(self.model, self.data, self.q, self.v) se3.updateFramePlacements(self.model, self.data) M = self.data.M h = self.data.nle self.collision_detection() self.compute_forces(False) if(self.simulate_coulomb_friction and self.simulation_type=='timestepping'): # minimize kinetic energy using time stepping from quadprog import solve_qp ''' Solve a strictly convex quadratic program Minimize 1/2 x^T G x - a^T x Subject to C.T x >= b Input Parameters ---------- G : array, shape=(n, n) a : array, shape=(n,) C : array, shape=(n, m) matrix defining the constraints b : array, shape=(m), default=None, vector defining the constraints meq : int, default=0 the first meq constraints are treated as equality constraints, all further as inequality constraints ''' # M (v' - v) = dt*S^T*(tau - tau_c) - dt*h + dt*J^T*f # M v' = M*v + dt*(S^T*tau - h + J^T*f) - dt*S^T*tau_c # M v' = b + B*tau_c # v' = Minv*(b + B*tau_c) b = M.dot(self.v) + dt*(self.S.T.dot(u) - h + self.Jc.T.dot(self.f)) B = - dt*self.S.T # Minimize kinetic energy: # min v'.T * M * v' # min (b+B*tau_c).T*Minv*(b+B*tau_c) # min tau_c.T * B.T*Minv*B* tau_C + 2*b.T*Minv*B*tau_c Minv = np.linalg.inv(M) G = B.T.dot(Minv.dot(B)) a = -b.T.dot(Minv.dot(B)) C = np.vstack((np.eye(self.na), -np.eye(self.na))) c = np.concatenate((-self.tau_coulomb_max, -self.tau_coulomb_max)) solution = solve_qp(G, a, C.T, c, 0) self.tau_c = solution[0] self.v = Minv.dot(b + B.dot(self.tau_c)) self.q = se3.integrate(self.model, self.q, self.v*dt) elif(self.simulation_type=='euler' or self.simulate_coulomb_friction==False): self.tau_c = self.tau_coulomb_max*np.sign(self.v[-self.na:]) self.dv = np.linalg.solve(M, self.S.T.dot(u-self.tau_c) - h + self.Jc.T.dot(self.f)) v_mean = self.v + 0.5*dt*self.dv self.v += self.dv*dt self.q = se3.integrate(self.model, self.q, v_mean*dt) else: print("[ERROR] Unknown simulation type:", self.simulation_type) self.t += dt return self.q, self.v
def callback_torques(): global v_prev, solo, stop, q, qdes, t t_start = time() 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) / h v_prev = v.copy() # Update display in Gepetto-gui solo.display(q) ######################################################################## ### Space mouse configuration : exit the loop when 0 is pressed and released event = sp.poll() # return the next event if there is any # if event signals the release of the first button # exit condition : the button 0 is pressed and released if type(event ) is sp.ButtonEvent and event.button == 0 and event.pressed == 0: # set exit condition stop = True ### Stack of Task : walking #compute/update all the joints and frames pin.forwardKinematics(solo.model, solo.data, qdes) pin.updateFramePlacements(solo.model, solo.data) # Getting the current height (on axis z) and the x-coordinate of the front left foot xz_FL = solo.data.oMf[ID_FL].translation[0::2] xz_FR = solo.data.oMf[ID_FR].translation[0::2] xz_HL = solo.data.oMf[ID_HL].translation[0::2] xz_HR = solo.data.oMf[ID_HR].translation[0::2] # Desired foot trajectory t1 = t #previous time t += dt t2 = t #current time xzdes_FL = ftraj(t, xF0, zF0) xzdes_HR = ftraj(t, xH0, zH0) xzdes_FR = ftraj(t + T / 2, xF0, zF0) xzdes_HL = ftraj(t + T / 2, xH0, zH0) # Calculating the error err_FL = xz_FL - xzdes_FL err_FR = xz_FR - xzdes_FR err_HL = xz_HL - xzdes_HL err_HR = xz_HR - xzdes_HR # Computing the local Jacobian into the global frame oR_FL = solo.data.oMf[ID_FL].rotation oR_FR = solo.data.oMf[ID_FR].rotation oR_HL = solo.data.oMf[ID_HL].rotation oR_HR = solo.data.oMf[ID_HR].rotation # Getting the different Jacobians fJ_FL3 = pin.frameJacobian(solo.model, solo.data, qdes, ID_FL)[:3, -8:] #Take only the translation terms oJ_FL3 = oR_FL * fJ_FL3 #Transformation from local frame to world frame oJ_FLxz = oJ_FL3[0::2, -8:] #Take the x and z components fJ_FR3 = pin.frameJacobian(solo.model, solo.data, qdes, ID_FR)[:3, -8:] oJ_FR3 = oR_FR * fJ_FR3 oJ_FRxz = oJ_FR3[0::2, -8:] fJ_HL3 = pin.frameJacobian(solo.model, solo.data, qdes, ID_HL)[:3, -8:] oJ_HL3 = oR_HL * fJ_HL3 oJ_HLxz = oJ_HL3[0::2, -8:] fJ_HR3 = pin.frameJacobian(solo.model, solo.data, qdes, ID_HR)[:3, -8:] oJ_HR3 = oR_HR * fJ_HR3 oJ_HRxz = oJ_HR3[0::2, -8:] # Displacement error nu = np.vstack([err_FL, err_FR, err_HL, err_HR]) # Making a single x&z-rows Jacobian vector J = np.vstack([oJ_FLxz, oJ_FRxz, oJ_HLxz, oJ_HRxz]) # Computing the velocity vq_act = -Kp * pinv(J) * nu vq = np.concatenate((np.zeros([6, 1]), vq_act)) # Computing the updated configuration qdes = pin.integrate(solo.model, qdes, vq * dt) #hist_err.append(np.linalg.norm(nu)) ######################################################################## # PD Torque controller Kp_PD = 0.05 Kd_PD = 120 * Kp_PD #Kd = 2 * np.sqrt(2*Kp) # formula to get a critical damping torques = Kd_PD * (qdes[7:] - q[7:]) - Kp_PD * v[6:] # Saturation to limit the maximal torque t_max = 5 torques = np.maximum(np.minimum(torques, t_max * np.ones((8, 1))), -t_max * np.ones((8, 1))) # Control loop of 1/dt Hz while (time() - t_start) < dt: sleep(10e-6) return torques, stop
def solve(self, dt, q_init, dq_init, com_ref, lmom_ref, amom_ref, endeff_pos_ref, endeff_vel_ref, endeff_contact, joint_pos_ref, base_ori_ref): num_time_steps = com_ref.shape[0] com_kin = np.zeros_like(com_ref) lmom_kin = np.zeros_like(lmom_ref) amom_kin = np.zeros_like(amom_ref) endeff_pos_kin = np.zeros_like(endeff_pos_ref) endeff_vel_kin = np.zeros_like(endeff_vel_ref) q_kin = np.zeros([num_time_steps, q_init.shape[0]]) dq_kin = np.zeros([num_time_steps, dq_init.shape[0]]) ddq_kin = np.zeros_like(dq_kin) inner_steps = int(dt / 0.001) inner_dt = 0.001 time = np.linspace(0., (num_time_steps - 1) * dt, num_time_steps) splined_com_ref = CubicSpline(time, com_ref) splined_lmom_ref = CubicSpline(time, lmom_ref) splined_amom_ref = CubicSpline(time, amom_ref) splined_endeff_pos_ref = CubicSpline(time, endeff_pos_ref) splined_endeff_vel_ref = CubicSpline(time, endeff_vel_ref) splined_joint_pos_ref = CubicSpline(time, joint_pos_ref) splined_base_ori_ref = CubicSpline(time, base_ori_ref) # store the first one q = q_init.copy() dq = dq_init.copy() self.update_kinematics(q, dq) q_kin[0] = q dq_kin[0] = dq com_kin[0] = self.robot.com(q).T hg = self.robot.centroidalMomentum(q, dq) lmom_kin[0] = hg.linear.T amom_kin[0] = hg.angular.T endeff_pos_kin[0] = self.framesPos(self.endeff_ids) endeff_vel_kin[0] = (self.J[6:(self.ne + 2) * 3].dot(dq).T).reshape( [self.ne, 3]) dmom_ref = np.zeros([ 6, ]) endeff_acc_ref = np.zeros([self.ne, 3]) t = 0. for it in range(1, num_time_steps): for inner in range(inner_steps): dmom_ref = np.hstack( (splined_lmom_ref(t, nu=1), splined_amom_ref(t, nu=1))) endeff_acc_ref = splined_endeff_vel_ref(t, nu=1) orien_ref = pin.Quaternion( pin.rpy.rpyToMatrix(splined_base_ori_ref(t))) ddq = self.step( q, dq, splined_com_ref(t), orien_ref, np.hstack((splined_lmom_ref(t), splined_amom_ref(t))), dmom_ref, splined_endeff_pos_ref(t), splined_endeff_vel_ref(t), endeff_acc_ref, endeff_contact[it], splined_joint_pos_ref(t)) # Integrate to the next state. dq += ddq * inner_dt q = pin.integrate(self.robot.model, q, dq * inner_dt) t += inner_dt self.update_kinematics(q, dq) q_kin[it] = q dq_kin[it] = dq ddq_kin[it] = ddq com_kin[it] = self.robot.com(q).T hg = self.robot.centroidalMomentum(q, dq) lmom_kin[it] = hg.linear.T amom_kin[it] = hg.angular.T endeff_pos_kin[it] = self.framesPos(self.endeff_ids) endeff_vel_kin[it] = (self.J[6:(self.ne + 2) * 3].dot(dq).T).reshape([self.ne, 3]) return q_kin, dq_kin, com_kin, lmom_kin, amom_kin, endeff_pos_kin, endeff_vel_kin
if i%PRINT_N == 0: print "Time %.3f"%(t) if invdyn.checkContact(contactRF.name, sol): f = invdyn.getContactForce(contactRF.name, sol) print "\tnormal force %s: %.1f"%(contactRF.name.ljust(20,'.'),contactRF.getNormalForce(f)) if invdyn.checkContact(contactLF.name, sol): f = invdyn.getContactForce(contactLF.name, sol) print "\tnormal force %s: %.1f"%(contactLF.name.ljust(20,'.'),contactLF.getNormalForce(f)) print "\ttracking err %s: %.3f"%(comTask.name.ljust(20,'.'), norm(comTask.position_error, 2)) print "\t||v||: %.3f\t ||dv||: %.3f"%(norm(v, 2), norm(dv)) v_mean = v + 0.5*dt*dv v += dt*dv q = se3.integrate(robot.model(), q, dt*v_mean) t += dt if i%DISPLAY_N == 0: robot_display.display(q) time_spent = time.time() - time_start if(time_spent < dt): time.sleep(dt-time_spent) # PLOT STUFF time = np.arange(0.0, N_SIMULATION*dt, dt) (f, ax) = plut.create_empty_figure(3,1) for i in range(3): ax[i].plot(time, com_pos[i,:].A1, label='CoM '+str(i)) ax[i].plot(time, com_pos_ref[i,:].A1, 'r:', label='CoM Ref '+str(i)) ax[i].set_xlabel('Time [s]')
def integrate_dv(self, q, v, dv, dt): v_mean = v + 0.5 * dt * dv v += dt * dv q = se3.integrate(self.model, q, dt * v_mean) return q, v
pin.forwardKinematics(robot.model, robot.data, q) pin.updateFramePlacements(robot.model, robot.data) Mtool = robot.data.oMf[IDX_TOOL] J = pin.frameJacobian(robot.model, robot.data, q, IDX_TOOL) nu = pin.log(Mtool.inverse() * Mgoal).vector vq = pinv(J)*nu q = pin.integrate(robot.model, q, vq * dt) robot.display(q) sleep(dt)""" ### Position the basis on the line robot.viewer.gui.addCylinder('world/yaxis', .01, 20, [.1, .1, .1, 1.]) place('world/yaxis', pin.SE3(rotate('x', np.pi / 2), np.matrix([0, 0, .1]).T)) IDX_BASIS = robot.model.getFrameId('base') q_basis = q for j in range(200): pin.forwardKinematics(robot.model, robot.data, q_basis) pin.updateFramePlacements(robot.model, robot.data) Mbasis = robot.data.oMf[IDX_BASIS] error = Mbasis.translation[0] - 1 J_basis = pin.frameJacobian(robot.model, robot.data, q, IDX_BASIS)[0, :] nu_basis = error vq_basis = pinv(J_basis) * nu_basis q_basis = pin.integrate(robot.model, q_basis, vq_basis * dt) robot.display(q_basis) #sleep(dt) embed()
def increment(self, q, v, dt, t, updateViewer=True): self.t = t self.time_step += 1 self.robot.v = v.copy() * dt self.q = se3.integrate(self.robot.model, q.copy(), self.robot.v) self.viewer.updateRobotConfig(self.q, self.robotName)