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))