def __init__(self, nb_legs_pairs, nn_dim, legs_pairs_dist_threshold, shoulders_dist_threshold, urdf = "/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo.urdf", modelPath="/opt/openrobots/share/example-robot-data/robots/solo_description/robots/"):
pin.switchToNumpyMatrix()
robot = pin.RobotWrapper.BuildFromURDF( urdf, modelPath, pin.JointModelFreeFlyer())
robot.initViewer(loadModel=True)
if ('viewer' in robot.viz.__dict__):
robot.viewer.gui.setRefreshIsSynchronous(False)
dt = 0.01
self.nbv = NonBlockingViewerFromRobot(robot,dt, nb_pairs=nb_legs_pairs, viz_thresh=3*legs_pairs_dist_threshold, act_thresh_legs=legs_pairs_dist_threshold, act_thresh_shd=shoulders_dist_threshold, shoulder_nn_dim=nn_dim)
def __init__(
self,
urdf="/opt/openrobots/lib/python3.5/site-packages/../../../share/example-robot-data/robots/solo_description/robots/solo.urdf",
modelPath="/opt/openrobots/lib/python3.5/site-packages/../../../share/example-robot-data/robots",
dt=0.01):
pin.switchToNumpyMatrix()
robot = pin.RobotWrapper.BuildFromURDF(urdf, modelPath,
pin.JointModelFreeFlyer())
robot.initDisplay(loadModel=True)
if ('viewer' in robot.viz.__dict__):
robot.viewer.gui.setRefreshIsSynchronous(False)
self.nbv = NonBlockingViewerFromRobot(robot, dt)
from math import sqrt
import numpy as np
import pinocchio as pin
from numpy.testing import assert_almost_equal as assertApprox
from rospkg import RosPack
import sot_talos_balance.talos.parameter_server_conf as param_server_conf
from sot_talos_balance.create_entities_utils import DcmController, create_parameter_server
pin.switchToNumpyMatrix()
# --- General ---
print("--- General ---")
dt = 0.001
robot_name = 'robot'
halfSitting = [
0.0,
0.0,
1.018213,
0.00,
0.0,
0.0,
1.0, # Free flyer
0.0,
0.0,
-0.411354,
0.859395,
-0.448041,
import sys
import unittest
from random import randint
import example_robot_data
import numpy as np
import crocoddyl
import pinocchio
from crocoddyl.utils import DifferentialFreeFwdDynamicsDerived, UnicycleDerived
pinocchio.switchToNumpyMatrix()
class ShootingProblemTestCase(unittest.TestCase):
MODEL = None
MODEL_DER = None
def setUp(self):
self.T = randint(1, 101)
state = self.MODEL.state
self.xs = []
self.us = []
self.xs.append(state.rand())
for i in range(self.T):
self.xs.append(state.rand())
self.us.append(np.matrix(np.random.rand(self.MODEL.nu)).T)
self.PROBLEM = crocoddyl.ShootingProblem(self.xs[0],
[self.MODEL] * self.T,
self.MODEL)
self.PROBLEM_DER = crocoddyl.ShootingProblem(self.xs[0],
import numpy as np
import pinocchio as se3
import tsid
print("")
print("Test Constraint Bound")
print("")
se3.switchToNumpyMatrix()
tol = 1e-5
n = 5
lb = np.matrix(-1.0 * np.ones(n)).transpose()
ub = np.matrix(np.ones(n)).transpose()
ConstBound = tsid.ConstraintBound("bounds", lb, ub)
assert ConstBound.isBound
assert not ConstBound.isEquality
assert not ConstBound.isInequality
assert ConstBound.rows == n
assert ConstBound.cols == n
assert lb.all() == ConstBound.lowerBound.all()
assert ub.all() == ConstBound.upperBound.all()
lb *= 2.0
assert np.linalg.norm(lb - ConstBound.lowerBound, 2) is not 0
ConstBound.setLowerBound(lb)
assert lb.all() == ConstBound.lowerBound.all()
'''
Inverse kinematics (close loop / iterative) for a mobile manipulator.
Template of the program for TP3
'''
import pinocchio as pio
from pinocchio.utils import *
import time
from numpy.linalg import pinv
from tiago_loader import loadTiago
pio.switchToNumpyMatrix()
import matplotlib.pylab as plt
plt.ion()
robot = loadTiago(initViewer=True)
gv = robot.viewer.gui
gv.setCameraTransform(0, [-8, -8, 2, .6, -0.25, -0.25, .7])
NQ = robot.model.nq
NV = robot.model.nv
IDX_TOOL = robot.model.getFrameId('frametool')
IDX_BASIS = robot.model.getFrameId('framebasis')
oMgoal = pio.SE3(
pio.Quaternion(-0.4, 0.02, -0.5, 0.7).normalized().matrix(),
np.matrix([.2, -.4, .7]).T)
gv.addXYZaxis('world/framegoal', [1., 0., 0., 1.], .015, .2)
gv.applyConfiguration('world/framegoal', list(pio.se3ToXYZQUAT(oMgoal).flat))