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