def computeCommand(t, q, v, sensors_data, u): # Verify sensor data. f = Force(sensors_data[jiminy.ForceSensor.type, "MassBody"], np.zeros(3)) f_joint_sensor = self.robot.pinocchio_model.frames[idx].placement * f f_jiminy = engine.system_state.f_external[self.robot.pinocchio_model.frames[idx].parent] self.assertTrue(np.allclose(f_joint_sensor.vector, f_jiminy.vector, atol=TOLERANCE)) u[:] = 0.0
def computeWrenchRef(res): Mcom = SE3.Identity() for k, t in enumerate(res.t_t): Mcom.translation = res.c_reference[:, k] Fcom = Force.Zero() Fcom.linear = cfg.MASS * (res.ddc_reference[:, k] - cfg.GRAVITY) Fcom.angular = res.dL_reference[:, k] F0 = Mcom.act(Fcom) res.wrench_reference[:, k] = F0.vector return res
def check_sensors_data(t, q, v, sensors_data, command): nonlocal engine, frame_pose # Verify sensor data, if the engine has been initialized if engine.is_initialized: contact_data = sensors_data[ jiminy.ContactSensor.type, self.body_name] f = Force(contact_data, np.zeros(3)) f_joint_sensor = frame_pose * f f_jiminy = engine.system_state.f_external[joint_idx] self.assertTrue(np.allclose( f_joint_sensor.vector, f_jiminy.vector, atol=TOLERANCE))
def appendZMP(first_iter_for_phase=False): 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])) wrench = phi0.vector zmp = shiftZMPtoFloorAltitude(cs, t, phi0, cfg.Robot) if first_iter_for_phase: phase.zmp_t = piecewise(polynomial(zmp.reshape(-1, 1), t, t)) phase.wrench_t = piecewise(polynomial(wrench.reshape(-1, 1), t, t)) else: phase.zmp_t.append(zmp, t) phase.wrench_t.append(wrench, t)
def computeZMPFromWrench(cs, Robot, dt): for phase in cs.contactPhases: t = phase.timeInitial phase.zmp_t = None while t <= phase.timeFinal: phi0 = Force(phase.wrench_t(t)) zmp = shiftZMPtoFloorAltitude(cs, t, phi0, Robot) if phase.zmp_t is None: phase.zmp_t = piecewise(polynomial(zmp.reshape(-1, 1), t, t)) else: phase.zmp_t.append(zmp, t) t += dt if phase.timeFinal - dt / 2. <= t <= phase.timeFinal + dt / 2.: t = phase.timeFinal
def computeZMPFromWrench(cs, time_t, wrench_t): N = len(time_t) ZMP_t = np.matrix(np.empty((3, N))) # smooth wrench traj : Wrench_trajectory = Trajectories.DifferentiableEuclidianTrajectory() Wrench_trajectory.computeFromPoints(np.asmatrix(time_t), wrench_t, 0 * wrench_t) for k in range(N): wrench = Wrench_trajectory(time_t[k])[0] phi0 = Force(wrench) ZMP_t[:, k] = shiftZMPtoFloorAltitude(cs, time_t[k], phi0) return ZMP_t
def appendZMP(first_iter_for_phase = False): """ Compute the zmp from the current wholebody data and append it to the current phase :param first_iter_for_phase: if True, initialize a new Curve for phase.zmp_t """ 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])) wrench = phi0.vector zmp = shiftZMPtoFloorAltitude(cs, t, phi0, cfg.Robot) if first_iter_for_phase: phase.zmp_t = piecewise(polynomial(zmp.reshape(-1,1), t, t)) phase.wrench_t = piecewise(polynomial(wrench.reshape(-1,1), t, t)) else: phase.zmp_t.append(zmp, t) phase.wrench_t.append(wrench, t)
def computeWrenchRef(cs, mass, G, dt): Mcom = SE3.Identity() for phase in cs.contactPhases: t = phase.timeInitial phase.wrench_t = None while t <= phase.timeFinal: Mcom.translation = phase.c_t(t) Fcom = Force.Zero() Fcom.linear = mass * (phase.ddc_t(t) - G) Fcom.angular = phase.dL_t(t) F0 = Mcom.act(Fcom) if phase.wrench_t is None: phase.wrench_t = piecewise( polynomial(F0.vector.reshape(-1, 1), t, t)) else: phase.wrench_t.append(F0.vector, t) t += dt if phase.timeFinal - dt / 2. <= t <= phase.timeFinal + dt / 2.: t = phase.timeFinal
def setRobotArgs(robot): ancestors.__defaults__ = (robot, ) + ancestors.__defaults__ descendants.__defaults__ = (robot, ) #ancestorsOf.__init__.__defaults__ = (robot,) iv.__defaults__ = (robot, ) parent.__defaults__ = (robot, ) jFromIdx.__defaults__ = (robot, ) # --- SE3 operators Mcross = lambda x, y: Motion(x).cross(Motion(y)).vector Mcross.__doc__ = "Motion cross product" Fcross = lambda x, y: Motion(x).cross(Force(y)).vector Fcross.__doc__ = "Force cross product" MCross = lambda V, v: np.bmat([Mcross(V[:, i], v) for i in range(V.shape[1])]) FCross = lambda V, f: np.bmat([Fcross(V[:, i], f) for i in range(V.shape[1])]) adj = lambda nu: np.bmat([[skew(nu[3:]), skew(nu[:3])], [zero([3, 3]), skew(nu[3:])]]) adj.__doc__ = "Motion pre-cross product (ie adjoint, lie bracket operator)" adjdual = lambda nu: np.bmat([[skew(nu[3:]), zero([3, 3])], [skew(nu[:3]), skew(nu[3:])]]) adjdual.__doc__ = "Force pre-cross product adjdual(a) = -adj(a)' " td = np.tensordot quad = lambda H, v: np.matrix(td(td(H, v, [2, 0]), v, [1, 0])).T
#################### """ 1 The scene, and later the movement and forces in the scene are modeled following Featherstone's Spatial Algebra. 2 Placement, i.e. rotation and translation of frames (and bodies) are stored in objects of the class SE3. 3 Rigid velocities and acceleration are stored in the class Motion, 4 Forces are stored in the class Forces 5 Masses/Inertias can be found in the class Inertias. An important point to note is that these objects store linear and angular part separately, but we often have to come back to a plain vector/matrix representation. In that case, contrary to Featherstone, we rather store linear part first and angular second. """ M = SE3.Random() nu = Motion.Random() phi = Force.Random() Y = Inertia.Random() print(M, nu, phi, Y) # As mentioned before, the linear and the angular components are stored separately. However we can easily get back to a vector representation. print(nu, nu.vector.T) # Pinocchio strictly separates the constant model element in the Model class, and all the buffers for storing algorithmic quantities # We can create a data buffer through... rdata = model.createData() # By default the RobotWrapper already createsa data buffer through.... robot.data # The idea is that the same model can be used by different part of the algorithm to compute different values from different argument. # For example, in a optimal-control implementation of Pinocchio, you likely want to have a single robot model for all your problem, # but several data for each node of your optimal control solver. ########################################################################## # Forward Kinematics
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