def loadTiago(initViewer=True):
'''Load the Tiago robot, and add two operation frames:
- at the front of the basis, named framebasis.
- at the end effector, name frametool.
Take care: Tiago as 3 continuous joints (one for the basis, two for the wheels), which makes
nq == nv+3.
'''
URDF_FILENAME = "tiago_no_hand.urdf"
URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = pio.RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH,
[modelPath], pio.JointModelPlanar())
robot.model.addFrame(
pio.Frame('framebasis', 1, 1,
pio.SE3(numpy.eye(3),
numpy.matrix([0.28, 0, 0.1]).T),
pio.FrameType.OP_FRAME))
robot.model.addFrame(
pio.Frame(
'frametool', 9, 51,
pio.SE3(numpy.matrix([[0, 1., 0], [0, 0, 1], [1, 0, 0]]),
numpy.matrix([0., 0, 0.06]).T), pio.FrameType.OP_FRAME))
robot.model.addFrame(
pio.Frame('framegaze', 11, 57,
pio.SE3(numpy.eye(3),
numpy.matrix([0.4, 0, 0]).T),
pio.FrameType.OP_FRAME))
robot.data = robot.model.createData()
jllow = robot.model.lowerPositionLimit
jllow[:2] = -2
robot.model.lowerPositionLimit = jllow
jlupp = robot.model.upperPositionLimit
jlupp[:2] = 2
robot.model.upperPositionLimit = jlupp
def display(robot, q):
robot.realdisplay(q)
pio.updateFramePlacements(robot.model, robot.viz.data)
robot.viewer.gui.applyConfiguration(
'world/' + robot.model.frames[-2].name,
list(pio.se3ToXYZQUAT(robot.viz.data.oMf[-2]).flat))
robot.viewer.gui.applyConfiguration(
'world/' + robot.model.frames[-1].name,
list(pio.se3ToXYZQUAT(robot.viz.data.oMf[-1]).flat))
robot.viewer.gui.refresh()
if initViewer:
robot.initViewer(loadModel=True)
gv = robot.viewer.gui
gv.addFloor('world/floor')
gv.addXYZaxis('world/framebasis', [1., 0., 0., 1.], .03, .1)
gv.addXYZaxis('world/frametool', [1., 0., 0., 1.], .03, .1)
#gv.addXYZaxis('world/framegaze', [1., 0., 0., 1.], .03, .1)
robot.realdisplay = robot.display
import types
robot.display = types.MethodType(display, robot)
return robot
def loadUR(robot=5, limited=False, gripper=False):
assert (not (gripper and (robot == 10 or limited)))
URDF_FILENAME = "ur%i%s_%s.urdf" % (robot, "_joint_limited" if limited else '', 'gripper' if gripper else 'robot')
URDF_SUBPATH = "/ur_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
model = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, ['/opt/openrobots/share/'])
if robot == 5 or robot == 3 and gripper:
SRDF_FILENAME = "ur%i%s.srdf" % (robot, '_gripper' if gripper else '')
SRDF_SUBPATH = "/ur_description/srdf/" + SRDF_FILENAME
readParamsFromSrdf(model, modelPath + SRDF_SUBPATH, False, False, None)
return model
def loadPendulum():
URDF_FILENAME = "pendulum.urdf"
URDF_SUBPATH = "/pendulum_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
return robot
v_max_scaling = 0.8
kp_contact = 10.0 # proportional gain of contact constraint
kp_foot = 10.0 # proportional gain of contact constraint
kp_com = 10.0 # proportional gain of center of mass task
kp_am = 10.0 # proportional gain of angular momentum task
kp_posture = 1.0 # proportional gain of joint posture task
viewer = pin.visualize.GepettoVisualizer
PRINT_N = 500 # print every PRINT_N time steps
DISPLAY_N = 20 # update robot configuration in viwewer every DISPLAY_N time steps
CAMERA_TRANSFORM = [
4.0, -0.2, 0.4, 0.5243823528289795, 0.518651008605957, 0.4620114266872406,
0.4925136864185333
]
SPHERE_RADIUS = 0.03
REF_SPHERE_RADIUS = 0.03
COM_SPHERE_COLOR = (1, 0.5, 0, 0.5)
COM_REF_SPHERE_COLOR = (1, 0, 0, 0.5)
RF_SPHERE_COLOR = (0, 1, 0, 0.5)
RF_REF_SPHERE_COLOR = (0, 1, 0.5, 0.5)
LF_SPHERE_COLOR = (0, 0, 1, 0.5)
LF_REF_SPHERE_COLOR = (0.5, 0, 1, 0.5)
urdf = "/talos_data/robots/talos_reduced.urdf"
path = getModelPath(urdf)
urdf = path + urdf
srdf = path + '/talos_data/srdf/talos.srdf'
path = os.path.join(path, '../..')
name = simu_params['name']
print("\nStart simulation", name)
if(simu_params['use_exp_int']):
data[name] = run_simulation(conf, dt, N, robot, controller, q0, v0, simu_params)
else:
data[name] = run_simulation(conf, dt, N, robot, controller, q0, v0, simu_params)
import consim_py.utils.visualize as visualize
from example_robot_data.robots_loader import getModelPath, getVisualPath
URDF_FILENAME = "solo12.urdf"
SRDF_FILENAME = "solo.srdf"
SRDF_SUBPATH = "/solo_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
contact_names = []
CameraTransform = [-0.015492438338696957,-3.359663963317871,0.5188663601875305,0.7071067690849304,
0.0,0.0,0.7071067690849304]
visualizer = visualize.Visualizer(showFloor=True, cameraTF=CameraTransform)
ground_truth_options = {'contact_names': ['HL_FOOT', 'HR_FOOT', 'FL_FOOT', 'FR_FOOT'],
'robot_color': [.3,1.,.3,.2],
'force_color': [0., 1., 0., .5],
'force_radius': .002,
'force_length': .025,
'cone_color': [0., 1., 0., .5],
'cone_length': .02,
'friction_coeff': .7
