def initDisplay(self, loadModel):
RobotWrapper.initDisplay(self, loadModel=loadModel)
if loadModel and not hasattr(self, 'display'):
RobotWrapper.initDisplay(self, loadModel=False)
try:
# self.viewer.gui.deleteNode('world/mobilebasis', True)
self.viewer.gui.addBox('world/mobilebasis', .25, .25, .25,
[.8, .2, .2, 1])
self.viewer.gui.addCylinder('world/mobilewheel1', .05, .45,
[.1, .0, .0, 1])
self.viewer.gui.addCylinder('world/mobilewheel2', .05, .45,
[.1, .0, .0, 1])
self.viewer.gui.setStaticTransform('world/mobilebasis',
[.0, .0, .35, 1.0, .0, .0, .0])
self.viewer.gui.setStaticTransform(
'world/mobilewheel1', [+0.15, .0, .05, .7, .7, .0, .0])
self.viewer.gui.setStaticTransform(
'world/mobilewheel2', [-0.15, .0, .05, .7, .7, .0, .0])
self.viewer.gui.addXYZaxis('world/framebasis', [1., 0., 0., 1.],
.03, .1)
self.viewer.gui.addXYZaxis('world/frametool', [1., 0., 0., 1.],
.03, .1)
except:
print("Error when adding 3d objects in the viewer ... ignore")
def __init__(self,*args):
filename = args[0]
if len(args) >= 2:
mesh_dir = args[1]
RobotWrapper.__init__(self,filename,mesh_dir,se3.JointModelFreeFlyer())
else:
RobotWrapper.__init__(self,filename,se3.JointModelFreeFlyer())
# RobotWrapper.__init__(self,filename, root_joint='ff')
self.q0 = np.matrix( [
0.0, 0.0, 0.648702, 0.0, 0.0 , 0.0, 1.0, # Free flyer 0-6
0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # LARM 11-17
0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # RARM 18-24
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
] ).T
self.ff = range(7)
self.chest = range(7,9)
self.head = range(9,11)
self.l_arm = range(11,18)
self.r_arm = range(18,25)
self.l_leg = range(25,31)
self.r_leg = range(31,37)
self.opCorrespondances = { "lh": "LARM_JOINT5",
"rh": "RARM_JOINT5",
"lf": "LLEG_JOINT5",
"rf": "RLEG_JOINT5",
}
for op,name in self.opCorrespondances.items():
idx = self.__dict__[op] = self.index(name)
def __init__(self,
filename=MODELPATH[0] +
'baxter_description/urdf/baxter.urdf'):
RobotWrapper.initFromURDF(self, filename, MODELPATH)
self.Q_INIT = np.zeros(self.nq - NQ_OFFSET)
self.q_def = np.zeros(self.nq)
self.v_def = np.zeros(self.nv)
# self.q0 = np.matrix( [
# 0.0, 0.0, 0.648702, 0.0, 0.0 , 0.0, 1.0, # Free flyer 0-6
# 0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
# 0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # LARM 11-17
# 0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # RARM 18-24
# 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30
# 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
# ] ).T
self.ff = list(range(7))
self.head = list(range(7, 7))
self.l_arm = list(range(8, 14))
self.r_arm = list(range(15, 21))
self.opCorrespondances = {
"lh": "left_w2",
"rh": "right_w2",
}
for op, name in list(self.opCorrespondances.items()):
idx = self.__dict__[op] = self.index(name)
def com(self, q=None, v=None, a=None, update=True):
if (update == False or q is None):
return PinocchioRobotWrapper.com(self, q)
if a is None:
if v is None:
return PinocchioRobotWrapper.com(self, q)
return PinocchioRobotWrapper.com(self, q, v)
return PinocchioRobotWrapper.com(self, q, v, a)
def loadRobot(M0, name):
'''
This function load a UR5 robot n a new model, move the basis to placement <M0>
and add the corresponding visuals in gepetto viewer with name prefix given by string <name>.
It returns the robot wrapper (model,data).
'''
robot = RobotWrapper(urdf, [PKG])
robot.model.jointPlacements[1] = M0 * robot.model.jointPlacements[1]
robot.visual_model.geometryObjects[0].placement = M0 * robot.visual_model.geometryObjects[0].placement
robot.visual_data.oMg[0] = M0 * robot.visual_data.oMg[0]
robot.initDisplay(loadModel=True, viewerRootNodeName="world/" + name)
return robot
def com(self, q=None, v=None, a=None, update_kinematics=True):
if(update_kinematics==False or q is None):
return PinocchioRobotWrapper.com(self, q);
if q is not None:
q_mat = np.asmatrix(q).reshape((self.model.nq,1))
if v is not None:
v_mat = np.asmatrix(v).reshape((self.model.nv,1))
if a is None:
if v is None:
return PinocchioRobotWrapper.com(self, q_mat)
return PinocchioRobotWrapper.com(self, q_mat, v_mat)
return PinocchioRobotWrapper.com(self, q_mat, v_mat, np.asmatrix(a).reshape((self.model.nv,1)))
def display(self, q):
RobotWrapper.display(self, q)
M1 = self.data.oMi[1]
self.viewer.gui.applyConfiguration('world/mobilebasis', se3ToXYZQUAT(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel1', se3ToXYZQUAT(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel2', se3ToXYZQUAT(M1))
self.viewer.gui.refresh()
pinocchio.updateFramePlacements(self.model, self.data)
self.viewer.gui.applyConfiguration('world/framebasis', se3ToXYZQUAT(self.data.oMf[-2]))
self.viewer.gui.applyConfiguration('world/frametool', se3ToXYZQUAT(self.data.oMf[-1]))
self.viewer.gui.refresh()
def display(self, q):
RobotWrapper.display(self, q)
M1 = self.data.oMi[1]
self.viewer.gui.applyConfiguration('world/mobilebasis', pin.se3ToXYZQUATtuple(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel1', pin.se3ToXYZQUATtuple(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel2', pin.se3ToXYZQUATtuple(M1))
self.viewer.gui.refresh()
pin.framesKinematics(self.model, self.data)
self.viewer.gui.applyConfiguration('world/framebasis', pin.se3ToXYZQUATtuple(self.data.oMf[-2]))
self.viewer.gui.applyConfiguration('world/frametool', pin.se3ToXYZQUATtuple(self.data.oMf[-1]))
self.viewer.gui.refresh()
def display(self, q):
RobotWrapper.display(self, q)
M1 = self.data.oMi[1]
self.viewer.gui.applyConfiguration('world/mobilebasis', M2gv(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel1', M2gv(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel2', M2gv(M1))
self.viewer.gui.refresh()
se3.framesKinematics(self.model, self.data)
self.viewer.gui.applyConfiguration('world/framebasis',
M2gv(self.data.oMf[-2]))
self.viewer.gui.applyConfiguration('world/frametool',
M2gv(self.data.oMf[-1]))
self.viewer.gui.refresh()
def __init__(self):
urdf_path = join(self.path, self.urdf_subpath, self.urdf_filename)
self.model_path = getModelPath(urdf_path, self.verbose)
self.urdf_path = join(self.model_path, urdf_path)
self.robot = RobotWrapper.BuildFromURDF(self.urdf_path, [join(self.model_path, '../..')],
pin.JointModelFreeFlyer() if self.free_flyer else None)
if self.srdf_filename:
self.srdf_path = join(self.model_path, self.path, self.srdf_subpath, self.srdf_filename)
self.robot.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path, self.verbose,
self.has_rotor_parameters, self.ref_posture)
if pin.WITH_HPP_FCL and pin.WITH_HPP_FCL_BINDINGS:
# Add all collision pairs
self.robot.collision_model.addAllCollisionPairs()
# Remove collision pairs per SRDF
pin.removeCollisionPairs(self.robot.model, self.robot.collision_model, self.srdf_path, False)
# Recreate collision data since the collision pairs changed
self.robot.collision_data = self.robot.collision_model.createData()
else:
self.srdf_path = None
self.robot.q0 = pin.neutral(self.robot.model)
if self.free_flyer:
self.addFreeFlyerJointLimits()
def loadANYmal(withArm=None):
if withArm is None:
URDF_FILENAME = "anymal.urdf"
SRDF_FILENAME = "anymal.srdf"
REF_POSTURE = "standing"
elif withArm == "kinova":
URDF_FILENAME = "anymal-kinova.urdf"
SRDF_FILENAME = "anymal-kinova.srdf"
REF_POSTURE = "standing_with_arm_up"
URDF_SUBPATH = "/anymal_b_simple_description/robots/" + URDF_FILENAME
SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH,
[getVisualPath(modelPath)],
pinocchio.JointModelFreeFlyer())
# Load SRDF file
robot.q0 = readParamsFromSrdf(robot.model,
modelPath + SRDF_SUBPATH,
False,
False,
referencePose=REF_POSTURE)
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot.model)
return robot
def loadHector():
URDF_FILENAME = "quadrotor_base.urdf"
URDF_SUBPATH = "/hector_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath],
pinocchio.JointModelFreeFlyer())
return robot
def initialize(self, planner_setting):
package_dirs = [
os.path.dirname(
os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
]
urdf = str(
os.path.dirname(
os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +
'/urdf/quadruped.urdf')
self.robot = RobotWrapper.BuildFromURDF(
urdf,
package_dirs=package_dirs,
root_joint=pin.JointModelFreeFlyer())
self.robot.data = self.robot.model.createData()
self.initDisplay(loadModel=True)
self.robot.viewer.gui.addFloor('world/floor')
self.z_floor = planner_setting.get(PlannerDoubleParam_FloorHeight)
self.robot.viewer.gui.applyConfiguration(
'world/floor', [0.0, 0.0, self.z_floor, 0.0, 0.0, 0.0, 1.0])
self.robot.viewer.gui.refresh()
self.robot.q = self.robot.model.referenceConfigurations.copy()
self.robot.dq = zero(self.robot.model.nv)
self.robot.q[7:] = np.transpose(
np.matrix(
planner_setting.get(
PlannerVectorParam_KinematicDefaultJointPositions)))
pin.forwardKinematics(self.robot.model, self.robot.data, self.robot.q)
self.robot.display(self.robot.q)
def buildRobotWrapper(cls):
# Rebuild the robot wrapper instead of using the existing model to
# also load the visuals.
robot = RobotWrapper.BuildFromURDF(cls.urdf_path, cls.meshes_path)
robot.model.rotorInertia[1:] = cls.motor_inertia
robot.model.rotorGearRatio[1:] = cls.motor_gear_ration
return robot
def loadRomeo():
URDF_FILENAME = "romeo.urdf"
URDF_SUBPATH = "/romeo_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH,
[getVisualPath(modelPath)],
pinocchio.JointModelFreeFlyer())
def __init__(self,log="/tmp/log_pickup.txt"):
self.wrapper = RobotWrapper("/local/mgeisert/models/ur5_quad.urdf")
self.client=Client()
self.client.gui.createWindow("w")
self.client.gui.createScene("world")
self.client.gui.addSceneToWindow("world",0L)
self.client.gui.addURDF("world/ur5","/local/mgeisert/models/ur5_quad.urdf","/local/mgeisert/models/universal_robot/")
self.client.gui.addSphere("world/sphere",0.07,[0.7,0.,0.,1.])
self.client.gui.createGroup("world/gripper")
self.client.gui.addLandmark("world/gripper",0.1)
self.wrapper.initDisplay("world/ur5")
self.list = []
file = open(log, "r")
configs = file.readlines()
list1 = []
tmp3 = 0.
for i in range(len(configs)):
tmp = np.fromstring(configs[i][2:], dtype=float, sep="\t")
tmp2 = []
if tmp[0]-tmp3>0.:
tmp2.extend([tmp[0]-tmp3])
tmp2.extend(tmp[1:4])
x = Quaternion().fromRPY(tmp[4],tmp[5],tmp[6])
tmp2.extend(x.array[1:4])
tmp2.extend([x.array[0]])
tmp2.extend(tmp[7:13])
tmp2.append(tmp[0])
tmp3 = tmp[0]
list1.append(tmp2)
self.list.append(list1)
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)
return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
def loadDoublePendulum():
URDF_FILENAME = "double_pendulum.urdf"
URDF_SUBPATH = "/double_pendulum_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH,
[getVisualPath(modelPath)])
return robot
def loadIris():
URDF_FILENAME = "iris_simple.urdf"
URDF_SUBPATH = "/iris_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath],
pinocchio.JointModelFreeFlyer())
return robot
def computeWrench(cs, Robot, dt):
rp = RosPack()
urdf = rp.get_path(
Robot.packageName
) + '/urdf/' + Robot.urdfName + Robot.urdfSuffix + '.urdf'
#srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf'
robot = RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
pin.JointModelFreeFlyer(), False)
model = robot.model
data = robot.data
q_t = cs.concatenateQtrajectories()
dq_t = cs.concatenateQtrajectories()
ddq_t = cs.concatenateQtrajectories()
t = q_t.min()
for phase in cs.contactPhases:
phase.wrench_t = None
t = phase.timeInitial
while t <= phase.timeFinal:
pin.rnea(model, data, phase.q_t(t), phase.dq_t(t), phase.ddq_t(t))
pcom, vcom, acom = robot.com(phase.q_t(t), phase.dq_t(t),
phase.ddq_t(t))
# FIXME : why do I need to call com to have the correct values in data ??
phi0 = data.oMi[1].act(pin.Force(data.tau[:6]))
if phase.wrench_t is None:
phase.wrench_t = piecewise(
polynomial(phi0.vector.reshape(-1, 1), t, t))
else:
phase.wrench_t.append(phi0.vector, t)
t += dt
if phase.timeFinal - dt / 2. <= t <= phase.timeFinal + dt / 2.:
t = phase.timeFinal
def main(robotpkg_prefix, robot):
pkg = Path(robotpkg_prefix) / 'share/example-robot-data'
urdf = ROBOTS[robot]
robot = RobotWrapper.BuildFromURDF(str(pkg / urdf), [str(pkg)],
pin.JointModelFreeFlyer())
robot.initDisplay()
robot.loadDisplayModel("pinocchio")
robot.display(robot.q0)
def loadPanda():
URDF_FILENAME = "panda.urdf"
URDF_SUBPATH = "/panda_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH,
[getVisualPath(modelPath)])
return robot
def __init__(self, urdf, pkgs):
RobotWrapper.__init__(self, urdf, pkgs, pinocchio.JointModelPlanar())
M0 = pinocchio.SE3(eye(3), np.matrix([0., 0., .6]).T)
self.model.jointPlacements[2] = M0 * self.model.jointPlacements[2]
self.visual_model.geometryObjects[0].placement = M0 * self.visual_model.geometryObjects[0].placement
self.visual_data.oMg[0] = M0 * self.visual_data.oMg[0]
# Placement of the "mobile" frame wrt basis center.
basisMop = pinocchio.SE3(eye(3), np.matrix([.3, .0, .1]).T)
self.model.addFrame(pinocchio.Frame('mobile', 1, 1, basisMop, pinocchio.FrameType.OP_FRAME))
# Placement of the tool frame wrt end effector frame (located at the center of the wrist)
effMop = pinocchio.SE3(eye(3), np.matrix([.0, .08, .095]).T)
self.model.addFrame(pinocchio.Frame('tool', 6, 6, effMop, pinocchio.FrameType.OP_FRAME))
# Create data again after setting frames
self.data = self.model.createData()
def on_connect(self, sid, environ):
self.user_sids.add(sid)
robot = RobotWrapper.BuildFromURDF(self.filename)
self.robot_map = self.robot_map.set(sid, robot)
self.q_map = self.q_map.set(sid, np.zeros(6))
F = cuda.device_array_like(np.zeros(tsdf_vol_params.shape))
W = cuda.device_array_like(np.zeros(tsdf_vol_params.shape))
self.tsdf_map = self.tsdf_map.set(sid, (F, W))
self.sync_vars_map = self.sync_vars_map.set(sid, dict())
def __init__(self):
filename = str(os.path.dirname(os.path.abspath(__file__)))
pkg = filename + '/../../Model'
urdf = pkg + '/jet_description/urdf/dyros_jet_robot.urdf'
self.robot = RobotWrapper.BuildFromURDF(urdf, [
pkg,
])
self.srdf = pkg + '/srdf/dyros_jet_robot.srdf'
self.model = self.robot.model
self.data = self.robot.data
def initDisplay(self, loadModel):
RobotWrapper.initDisplay(self, loadModel=loadModel)
if loadModel and not hasattr(self, 'display'):
RobotWrapper.initDisplay(self, loadModel=False)
try:
# self.viewer.gui.deleteNode('world/mobilebasis', True)
self.viewer.gui.addBox('world/mobilebasis', .25, .25, .25, [.8, .2, .2, 1])
self.viewer.gui.addCylinder('world/mobilewheel1', .05, .45, [.1, .0, .0, 1])
self.viewer.gui.addCylinder('world/mobilewheel2', .05, .45, [.1, .0, .0, 1])
self.viewer.gui.setStaticTransform('world/mobilebasis', [.0, .0, .35, 1.0, .0, .0, .0])
self.viewer.gui.setStaticTransform('world/mobilewheel1', [+0.15, .0, .05, .7, .7, .0, .0])
self.viewer.gui.setStaticTransform('world/mobilewheel2', [-0.15, .0, .05, .7, .7, .0, .0])
self.viewer.gui.addXYZaxis('world/framebasis', [1., 0., 0., 1.], .03, .1)
self.viewer.gui.addXYZaxis('world/frametool', [1., 0., 0., 1.], .03, .1)
except:
print("Error when adding 3d objects in the viewer ... ignore")
def __init__(self):
self.PKG = os.path.dirname(os.path.abspath(__file__)) + '/../../../resources/urdfs/anymal/'
self.URDF = self.PKG + 'urdf/anymal_boxy.urdf'
if self.PKG is None:
self.robot = RobotWrapper.BuildFromURDF(self.URDF)
else:
self.robot = RobotWrapper.BuildFromURDF(self.URDF, [self.PKG])
self.robot = RobotWrapper.BuildFromURDF(self.URDF, [self.PKG])
self.model = self.robot.model
self.data = self.robot.data
self.LF_foot_jac = None
self.LH_foot_jac = None
self.RF_foot_jac = None
self.RH_foot_jac = None
self.urdf_foot_name_lf = 'LF_FOOT'
self.urdf_foot_name_lh = 'LH_FOOT'
self.urdf_foot_name_rf = 'RF_FOOT'
self.urdf_foot_name_rh = 'RH_FOOT'
self.ik_success = False
def loadModels(robotname, robot_urdf_file):
"""This function create a robot model and its data by inputting a URDF file that describes the robot.
Input: robotname: directory containing model of robot
robot_urdf_file: URDF file of robot
Output: robot: robot model and data created by Pinocchio"""
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),
"models")
model_path = join(pinocchio_model_dir, "others/robots")
mesh_dir = model_path
urdf_filename = robot_urdf_file
urdf_dir = robotname + "/urdf"
urdf_model_path = join(join(model_path, urdf_dir), urdf_filename)
if not isFext:
robot = RobotWrapper.BuildFromURDF(urdf_model_path, mesh_dir)
else:
robot = RobotWrapper.BuildFromURDF(urdf_model_path, mesh_dir,
pin.JointModelFreeFlyer())
return robot
def __init__(self):
current_dir = os.path.dirname(__file__)
robot_dir = os.path.join(current_dir, "assets/")
urdf_path = os.path.join(current_dir, "assets/poppy_ergo_jr.urdf")
self.wrapper = RobotWrapper.BuildFromURDF(urdf_path,
package_dirs=[robot_dir])
self.dummy_data = self.wrapper.data.copy()
self._set_collision_pairs(self.wrapper)
# do not count the universe as a joint
self.joints_shape = (self.wrapper.model.njoints - 1, 3)
def __init__(self,filename):
RobotWrapper.__init__(self,filename)
a=-0.001616164386097708
self.q0= np.matrix( [
0, 0, 0.817, 0, 0, 0, 1, # Free flyer
0, 0.05, -0.40+a, 0.75, -0.35-a, -0.05, # left leg
0, -0.05, -0.40+a, 0.75, -0.35-a, 0.05, # right leg
0,0, # chest
0, 0.15, 0, 0.3, 0, 0.5, 0, # left arm
0, 0.15, 0, 0.3, 0, 0.5, 0, # right arm
0.0, 0.0 # head
] ).T
self.opCorrespondances = { "lh": "arm_left_7_joint",
"rh": "arm_right_7_joint",
"rf": "leg_right_6_joint",
"lf": "leg_left_6_joint",
}
for op,name in self.opCorrespondances.items():
idx = self.__dict__[op] = self.index(name)
def __init__(self):
self.PKG = os.path.dirname(
os.path.abspath(__file__)) + '/../../resources/urdfs/hyqreal/'
self.URDF = self.PKG + 'urdf/hyqreal.urdf'
if self.PKG is None:
self.robot = RobotWrapper.BuildFromURDF(self.URDF)
else:
self.robot = RobotWrapper.BuildFromURDF(self.URDF, [self.PKG])
self.model = self.robot.model
self.data = self.robot.data
self.LF_foot_jac = None
self.LH_foot_jac = None
self.RF_foot_jac = None
self.RH_foot_jac = None
self.urdf_foot_name_lf = 'lf_foot'
self.urdf_foot_name_lh = 'lh_foot'
self.urdf_foot_name_rf = 'rf_foot'
self.urdf_foot_name_rh = 'rh_foot'
def loadHyQ(modelPath=None):
URDF_FILENAME = "hyq_no_sensors.urdf"
URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
if modelPath is None:
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# TODO define default position inside srdf
robot.q0.flat[7:] = [-0.2, 0.75, -1.5, -0.2, -0.75, 1.5, -0.2, 0.75, -1.5, -0.2, -0.75, 1.5]
robot.q0[2] = 0.57750958
robot.model.referenceConfigurations["half_sitting"] = robot.q0
return robot
def loadKinova():
URDF_FILENAME = "kinova.urdf"
SRDF_FILENAME = "kinova.srdf"
SRDF_SUBPATH = "/kinova_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/kinova_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "arm_up")
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 loadTiagoNoHand():
URDF_FILENAME = "tiago_no_hand.urdf"
# SRDF_FILENAME = "tiago.srdf"
# SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
# Load SRDF file
# readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
return robot
'chest' : 'TrunkYaw'}
_initialConfig = (0, 0, .840252, 0, 0, 0, # FF
0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # LLEG
0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # RLEG
0, # TRUNK
1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, # LARM
0, 0, 0, 0, # HEAD
1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2, # RARM
)
#----------------PINOCCHIO----------------------------#
import pinocchio as se3
from pinocchio.robot_wrapper import RobotWrapper
pinocchioRobot = RobotWrapper(_urdfPath, _urdfDir, se3.JointModelFreeFlyer())
pinocchioRobot.initDisplay(loadModel=True)
#----------------ROBOT - DEVICE AND DYNAMICS----------#
from dynamic_graph.sot.dynamics.humanoid_robot import HumanoidRobot
robot = HumanoidRobot(_robotName, pinocchioRobot.model,
pinocchioRobot.data, _initialConfig, _OperationalPointsMap)
#-----------------------------------------------------#
#----------------SOT (SOLVER)-------------------------#
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
solver = initialize( robot )
#-----------------------------------------------------#
class PlayFromFile:
def __init__(self,log="/tmp/log_pickup.txt"):
self.wrapper = RobotWrapper("/local/mgeisert/models/ur5_quad.urdf")
self.client=Client()
self.client.gui.createWindow("w")
self.client.gui.createScene("world")
self.client.gui.addSceneToWindow("world",0L)
self.client.gui.addURDF("world/ur5","/local/mgeisert/models/ur5_quad.urdf","/local/mgeisert/models/universal_robot/")
self.client.gui.addSphere("world/sphere",0.07,[0.7,0.,0.,1.])
self.client.gui.createGroup("world/gripper")
self.client.gui.addLandmark("world/gripper",0.1)
self.wrapper.initDisplay("world/ur5")
self.list = []
file = open(log, "r")
configs = file.readlines()
list1 = []
tmp3 = 0.
for i in range(len(configs)):
tmp = np.fromstring(configs[i][2:], dtype=float, sep="\t")
tmp2 = []
if tmp[0]-tmp3>0.:
tmp2.extend([tmp[0]-tmp3])
tmp2.extend(tmp[1:4])
x = Quaternion().fromRPY(tmp[4],tmp[5],tmp[6])
tmp2.extend(x.array[1:4])
tmp2.extend([x.array[0]])
tmp2.extend(tmp[7:13])
tmp2.append(tmp[0])
tmp3 = tmp[0]
list1.append(tmp2)
self.list.append(list1)
def play(self, i=0, speed=1):
for config in self.list[i]:
q = np.matrix([[config[1]],[config[2]],[config[3]],[config[4]],[config[5]],[config[6]],[config[7]],[config[8]],[config[9]],[config[10]],[config[11]],[config[12]],[config[13]]])
self.wrapper.display(q)
x = np.matrix([[0],[0.15],[0]])
x = self.wrapper.data.oMi[7]*x
x = x.transpose()
x = x.tolist()[0]
print x
quat = Quaternion(self.wrapper.data.oMi[7].rotation)
x.extend(quat.array[0:4])
self.client.gui.applyConfiguration("world/gripper",x)
if config[14]<3:
self.client.gui.applyConfiguration("world/sphere",[2,-1,-1,1,0,0,0])
elif config[14]<7:
self.client.gui.applyConfiguration("world/sphere",x)
self.client.gui.refresh()
time.sleep(config[0]*speed)
def display(self,k, i=0):
config = self.list[i][k]
q = np.matrix([[config[1]],[config[2]],[config[3]],[config[4]],[config[5]],[config[6]],[config[7]],[config[8]],[config[9]],[config[10]],[config[11]],[config[12]],[config[13]]])
self.wrapper.display(q)
def reload(self,file="/tmp/log_pickup.txt"):
file = open(file, "r")
configs = file.readlines()
list1 = []
tmp3 = 0.
for i in range(len(configs)):
tmp = np.fromstring(configs[i][2:], dtype=float, sep="\t")
tmp2 = []
if tmp[0]-tmp3>0.:
tmp2.extend([tmp[0]-tmp3])
tmp2.extend(tmp[1:4])
x = Quaternion().fromRPY(tmp[4],tmp[5],tmp[6])
tmp2.extend(x.array[1:4])
tmp2.extend([x.array[0]])
tmp2.extend(tmp[7:13])
tmp2.append(tmp[0])
tmp3 = tmp[0]
list1.append(tmp2)
self.list.insert(0,list1)
0., 0. # Head
)
#-----------------------------------------------------------------------------
#---- ROBOT SPECIFICATIONS----------------------------------------------------
#-----------------------------------------------------------------------------
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
from pinocchio.robot_wrapper import RobotWrapper
import pinocchio as pin
from dynamic_graph.sot.dynamics_pinocchio import fromSotToPinocchio
pinocchioRobot = RobotWrapper(urdfPath, urdfDir, pin.JointModelFreeFlyer())
pinocchioRobot.initDisplay(loadModel=True)
pinocchioRobot.display(fromSotToPinocchio(initialConfig))
from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import HumanoidRobot
robot = HumanoidRobot(robotName, pinocchioRobot.model,
pinocchioRobot.data, initialConfig, OperationalPointsMap)
# ------------------------------------------------------------------------------
# ---- Kinematic Stack of Tasks (SoT) -----------------------------------------
# ------------------------------------------------------------------------------
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
sot = SOT('sot')
