import pinocchio as pin
import numpy as np
import sys
from os.path import dirname, join, abspath
# add path to the example-robot-data package
path = join(dirname(dirname(abspath(__file__))), 'models', 'others', 'python')
sys.path.append(path)
from example_robot_data import loadTalos
# import visualizer
from panda3d_viewer import ViewerClosedError
from pinocchio.visualize.panda3d_visualizer import Panda3dVisualizer
talos = loadTalos()
talos.setVisualizer(Panda3dVisualizer())
talos.initViewer()
talos.loadViewerModel(group_name='talos', color=(1, 1, 1, 1))
# Play a sample trajectory in a loop
def play_sample_trajectory():
update_rate = 60
cycle_time = 3
traj = np.repeat(talos.q0.reshape((-1, 1)),
cycle_time * update_rate,
axis=1)
beta = np.linspace(0, 1, traj.shape[1])
traj[[2, 9, 10, 11, 22, 15, 16, 17, 30]] = (
0.39 + 0.685 * np.cos(beta),
class TalosFloatingBaseActuationTest(ActuationModelAbstractTestCase):
STATE = crocoddyl.StateMultibody(example_robot_data.loadTalos().model)
ACTUATION = crocoddyl.ActuationModelFloatingBase(STATE)
ACTUATION_DER = FreeFloatingActuationDerived(STATE)
import numpy as np
import pinocchio as pin
from example_robot_data import loadTalos
SCALE = 0.01
TXT_FILE = 'talos_dist{}.txt'.format(SCALE)
READ_FROM_TXT = False
PLOT_DISPARITY = False
# retrieve the normal Talos model
r = loadTalos()
rmodel = r.model
rdata = r.data
# create a brand new model object (discard the data)
rmodel_dist = loadTalos().model
if READ_FROM_TXT:
rmodel_dist.loadFromText(TXT_FILE)
else:
for iner in rmodel_dist.inertias:
iner.lever += SCALE * (np.random.rand(3) - 0.5)
rdata_dist = rmodel_dist.createData()
# CoM
c = pin.centerOfMass(rmodel, rdata, r.q0)
c_dist = pin.centerOfMass(rmodel_dist, rdata_dist, r.q0)
print('c_dist - c')
print(c_dist - c)
# Mass matrix
c_arr = np.array([c_traj(t) for t in t_arr])
dc_arr = np.array([dc_traj(t) for t in t_arr])
Lc_arr = np.array([Lc_traj(t) for t in t_arr])
f_arr_lst = [np.array([f_traj_lst[i](t) for t in t_arr]) for i in range(len(contact_frames))]
# l_wrench_lst = [f12TOphi(f12) for f12 in f_arr_lst[0]]
# r_wrench_lst = [f12TOphi(f12) for f12 in f_arr_lst[1]]
l_wrench_lst = f_arr_lst[0]
r_wrench_lst = f_arr_lst[1]
N = t_arr.shape[0] # nb of discretized timestamps
def clip(i):
return min(max(0,i), N-1)
# Load robot model
robot = loadTalos()
rmodel = robot.model
rdata = robot.data
contact_frame_ids = [rmodel.getFrameId(l) for l in contact_frames]
print(contact_frame_ids)
robot.com(robot.q0)
mass = rdata.mass[0]
print('mass talos: ', mass)
gravity = rmodel.gravity.linear
# initialize
oRb = pin.Quaternion(q_arr[0,3:7].reshape((4,1))).toRotationMatrix()
# init est and gtr lists
p_est_lst = []
'''
f0 = copy.copy(func(q))
fs = []
v = np.zeros(nv)
for k in range(nv):
v[k] = eps
qk = exp(q, v)
fs.append((func(qk) - f0) / eps)
v[k] -= eps
if isinstance(fs[0], np.ndarray) and len(fs[0]) > 1:
return np.stack(fs, axis=1)
else:
return np.array(fs)
# Num diff checking, for each cost.
robot = robex.loadTalos()
q = pin.randomConfiguration(robot.model)
# Tdiffq is used to compute the tangent application in the configuration space.
def Tdiffq(f, q):
return Tdiff1(f, lambda q, v: pin.integrate(robot.model, q, v),
robot.model.nv, q)
# Test Cost3d
CostClass = Cost3d
cost = CostClass(robot.model, robot.data)
Tg = cost.calcDiff(q)
Tgn = Tdiffq(cost.calc, q)
assert norm(Tg - Tgn) < 1e-4
class StateMultibodyTalosTest(StateAbstractTestCase):
MODEL = example_robot_data.loadTalos().model
NX = MODEL.nq + MODEL.nv
NDX = 2 * MODEL.nv
STATE = crocoddyl.StateMultibody(MODEL)
STATE_DER = StateMultibodyDerived(MODEL)
class TalosTest(RobotTestCase):
RobotTestCase.ROBOT = example_robot_data.loadTalos()
RobotTestCase.NQ = 39
RobotTestCase.NV = 38
import os import sys import crocoddyl from crocoddyl.utils.biped import plotSolution import numpy as np import example_robot_data import pinocchio WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ # Load robot robot = example_robot_data.loadTalos() rmodel = robot.model # Create data structures rdata = rmodel.createData() state = crocoddyl.StateMultibody(rmodel) actuation = crocoddyl.ActuationModelFloatingBase(state) # Set integration time DT = 5e-2 T = 60 target = np.array([0.4, 0, 1.2]) # Initialize reference state, target and reference CoM rightFoot = 'right_sole_link' leftFoot = 'left_sole_link' endEffector = 'gripper_left_joint' endEffectorId = rmodel.getFrameId(endEffector)
def configure_simulation(dt, enableGUI):
global jointTorques
# Load robot
robot = loadTalos()
robot.initDisplay(loadModel=True)
rmodel = robot.model
# Defining the initial state of the robot
q0 = robot.model.referenceConfigurations['half_sitting'].copy()
q0[2] = 1.2 # Note that pinocchio's index is as follow: left leg (7), right leg, torso(2), left arm(8), right arm, head(2)
v0 = pinocchio.utils.zero(robot.model.nv)
nq = robot.model.nq
nv = robot.model.nv
na = nq - 7 # actual activated joint
# Start the client for PyBullet
if enableGUI:
physicsClient = p.connect(p.GUI)
else:
physicsClient = p.connect(p.DIRECT) # noqa
# p.GUI for graphical version
# p.DIRECT for non-graphical version
# Set gravity (disabled by default)
p.setGravity(0, 0, -9.81)
# Load horizontal plane for PyBullet
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
# Load the robot for PyBullet
robotStartPos = q0[:3]
robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
p.setAdditionalSearchPath(
"/opt/openrobots/share/example-robot-data/robots/talos_data/robots")
robotId = p.loadURDF("talos_reduced.urdf", robotStartPos,
robotStartOrientation)
# Set time step of the simulation
# dt = 0.001
p.setTimeStep(dt)
# realTimeSimulation = True # If True then we will sleep in the main loop to have a frequency of 1/dt
# set Initial Posture
revoluteJointIndices = np.concatenate(
(np.arange(45, 51), np.arange(52, 58), np.arange(
0, 2), np.arange(11, 19), np.arange(28, 36), np.arange(3, 5)),
axis=None)
for m in range(na):
p.resetJointState(robotId, revoluteJointIndices[m], q0[m + 7])
# Disable default motor control for revolute joints
p.setJointMotorControlArray(
robotId,
jointIndices=revoluteJointIndices,
controlMode=p.VELOCITY_CONTROL,
targetVelocities=[0.0 for m in revoluteJointIndices],
forces=[0.0 for m in revoluteJointIndices])
# Enable torque control for revolute joints
jointTorques = [0.0 for m in revoluteJointIndices]
p.setJointMotorControlArray(robotId,
revoluteJointIndices,
controlMode=p.TORQUE_CONTROL,
forces=jointTorques)
# Compute one step of simulation for initialization
p.stepSimulation()
return robotId, robot, revoluteJointIndices
Some variations around the computations of the centroidal momentum.
Conclusion:
use pio.computeCentroidalMap(rmodel,rdata,q) to get the matrix c_Ag s.t.:
c_Ag * vq = c_Ag * [ b_v_b,b_w,qdot_A ]
= [ m*0_cd, 0_Lc ]
= [ m*0_cd, 0_Ic*0_w+Aq*qdot_A ]
'''
import numpy as np
import pinocchio as pio
from example_robot_data import loadTalos
np.set_printoptions(precision=1, threshold=1e4, suppress=True, linewidth=200)
from numpy.linalg import norm, svd, eig, inv, pinv
r = loadTalos()
rm = r.model
rd = rm.createData()
q0 = r.q0.copy()
q = pio.randomConfiguration(rm)
vq = np.random.rand(rm.nv) * 2 - 1
b_Mc = pio.crba(rm, rd, q) # mass matrix at b frame expressed in b frame
b_I_b = b_Mc[:6, :6] # inertia matrix at b frame expressed in b frame
Jc = pio.jacobianCenterOfMass(rm, rd, q)
w_T_b = rd.oMi[1] # world to body transform
# Check that the Inertia matrix expressed at the center of mass is of the form:
# c_I_c = b_Mc[:6,:6] = [[m*Id3 03 ]
