def initializeUrRobot(self):
"""
initialize ur robot
"""
if not self.dynamic:
raise RunTimeError("robots models have to be initialized first")
if not self.device:
self.device = RobotSimu(self.name + '_device')
plug(self.device.state, self.dynamic.position)
self.dynamic.velocity.value = self.dimension * (0., )
self.dynamic.acceleration.value = self.dimension * (0., )
self.initializeOpPoints(self.dynamic)
def initializeRobot(self):
"""
If the robot model is correctly loaded, this method will then
initialize the operational points, set the position to
half-sitting with null velocity/acceleration.
To finish, different tasks are initialized:
- the center of mass task used to keep the robot stability
- one task per operational point to ease robot control
"""
if not self.dynamic:
raise RunTimeError("robots models have to be initialized first")
if not self.device:
self.device = RobotSimu(self.name + '_device')
"""
Robot timestep
"""
self.timeStep = self.device.getTimeStep()
# Freeflyer reference frame should be the same as global
# frame so that operational point positions correspond to
# position in freeflyer frame.
self.device.set(self.halfSitting)
plug(self.device.state, self.dynamic.position)
if self.enableVelocityDerivator:
self.velocityDerivator = Derivator_of_Vector('velocityDerivator')
self.velocityDerivator.dt.value = self.timeStep
plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity)
else:
self.dynamic.velocity.value = self.dimension*(0.,)
if self.enableAccelerationDerivator:
self.accelerationDerivator = \
Derivator_of_Vector('accelerationDerivator')
self.accelerationDerivator.dt.value = self.timeStep
plug(self.velocityDerivator.sout,
self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
else:
self.dynamic.acceleration.value = self.dimension*(0.,)
def __init__(self,
name,
pinocchio_model,
pinocchio_data,
initialConfig,
OperationalPointsMap=None,
tracer=None):
AbstractHumanoidRobot.__init__(self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.OperationalPointsMap = OperationalPointsMap
self.dynamic = DynamicPinocchio(self.name + "_dynamic")
self.dynamic.setModel(pinocchio_model)
self.dynamic.setData(pinocchio_data)
self.dimension = self.dynamic.getDimension()
self.device = RobotSimu(self.name + "_device")
self.device.resize(self.dynamic.getDimension())
self.halfSitting = initialConfig
self.device.set(self.halfSitting)
plug(self.device.state, self.dynamic.position)
if self.enableVelocityDerivator:
self.velocityDerivator = Derivator_of_Vector('velocityDerivator')
self.velocityDerivator.dt.value = self.timeStep
plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity)
else:
self.dynamic.velocity.value = self.dimension * (0., )
if self.enableAccelerationDerivator:
self.accelerationDerivator = \
Derivator_of_Vector('accelerationDerivator')
self.accelerationDerivator.dt.value = self.timeStep
plug(self.velocityDerivator.sout, self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
else:
self.dynamic.acceleration.value = self.dimension * (0., )
if self.OperationalPointsMap is not None:
self.initializeOpPoints()
def initializeUrRobot(self):
"""
initialize ur robot
"""
if not self.dynamic:
raise RunTimeError("robots models have to be initialized first")
if not self.device:
self.device = RobotSimu(self.name + '_device')
plug(self.device.state, self.dynamic.position)
self.dynamic.velocity.value = self.dimension * (0., )
self.dynamic.acceleration.value = self.dimension * (0., )
self.initializeOpPoints(self.dynamic)
def __init__(self, name, device=None, tracer=None):
AbstractHumanoidRobot.__init__(self, name, tracer)
self.device = device
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
#self.specifySpecialLinks()
self.dynamic.loadFromParameterServer()
self.dimension = self.dynamic.getDimension()
self.initializeUrRobot()
__all__ = ["Ur"]
##########################
####### demo code ########
##########################
robot = Ur('Ur5', device=RobotSimu('Ur5'))
self.initPosition = (0., ) * self.dimension
# initialize ur robot
self.initializeRobot()
__all__ = ["Ur"]
#### demo code ####
# 1. Instanciate a Pr2
# The URDF description of the robot must have
# been loaded in robot_description parameter
# on the Ros Parameter Server
from dynamic_graph.sot.pr2.robot import Pr2
from dynamic_graph.sot.core import RobotSimu
from dynamic_graph import plug
robot = youbot('youbot', device=RobotSimu('youbot'))
plug(robot.device.state, robot.dynamic.position)
# 2. Ros binding
# roscore must be running
from dynamic_graph.ros import Ros
ros = Ros(robot)
# 3. Create a solver
from dynamic_graph.sot.application.velocity.precomputed_tasks import Solver
solver = Solver(robot)
# 4. Define a position task for the right hand
from dynamic_graph.sot.core.meta_tasks_kine import gotoNd, MetaTaskKine6d
from numpy import eye
from dynamic_graph.sot.core.matrix_util import matrixToTuple
def initializeRobot(self):
"""
If the robot model is correctly loaded, this method will then
initialize the operational points, set the position to
half-sitting with null velocity/acceleration.
To finish, different tasks are initialized:
- the center of mass task used to keep the robot stability
- one task per operational point to ease robot control
"""
if not self.dynamic:
raise RunTimeError("robots models have to be initialized first")
if not self.device:
self.device = RobotSimu(self.name + '_device')
# Freeflyer reference frame should be the same as global
# frame so that operational point positions correspond to
# position in freeflyer frame.
self.device.set(self.halfSitting)
plug(self.device.state, self.dynamic.position)
if self.enableVelocityDerivator:
self.velocityDerivator = Derivator_of_Vector('velocityDerivator')
self.velocityDerivator.dt.value = self.timeStep
plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity)
else:
self.dynamic.velocity.value = self.dimension*(0.,)
if self.enableAccelerationDerivator:
self.accelerationDerivator = \
Derivator_of_Vector('accelerationDerivator')
self.accelerationDerivator.dt.value = self.timeStep
plug(self.velocityDerivator.sout,
self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
else:
self.dynamic.acceleration.value = self.dimension*(0.,)
self.initializeOpPoints(self.dynamic)
# --- additional frames ---
self.frames = dict()
frameName = 'rightHand'
hp = self.dynamic.getHandParameter (True)
transformation = list (map (list, I4))
for i in range (3): transformation [i][3] = hp [i][3]
transformation = tuple (map (tuple, transformation))
self.frames [frameName] = self.createFrame (
"{0}_{1}".format (self.name, frameName),
transformation, "right-wrist")
frameName = 'leftHand'
hp = self.dynamic.getHandParameter (False)
transformation = list (map (list, I4))
for i in range (3): transformation [i][3] = hp [i][3]
transformation = tuple (map (tuple, transformation))
self.frames [frameName] = self.createFrame (
"{0}_{1}".format (self.name, frameName),
self.dynamic.getHandParameter (False), "left-wrist")
for (frameName, transformation, signalName) in self.AdditionalFrames:
self.frames[frameName] = self.createFrame(
"{0}_{1}".format(self.name, frameName),
transformation,
signalName)
# -*- coding: utf-8 -*-
"""
Created on Mon Oct 21 11:34:27 2013
@author: bcoudrin
"""
from dynamic_graph import plug
from dynamic_graph.sot.core import RobotSimu
from dynamic_graph.sot.ur.robot import Ur
from dynamic_graph.sot.dyninv import SolverKine
from dynamic_graph.sot.core.meta_task_6d import toFlags
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d
from dynamic_graph.sot.dyninv import TaskJointLimits
#from dynamic_graph.ros.ros_sot_robot_model import Ros
robot = Ur('Ur', device=RobotSimu('ur'))
plug(robot.device.state, robot.dynamic.position)
#ros = Ros(robot)
def toList(sot):
return map(lambda x: x[1:-1], sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robot.dimension)
robot.dynamic.velocity.value = robot.dimension * (0., )
robot.dynamic.acceleration.value = robot.dimension * (0., )
robot.dynamic.ffposition.unplug()
# 0. TRICK: import Dynamic as the first command to avoid the crash at the exit
from dynamic_graph.sot.dynamics import Dynamic
# 1. Instanciate a Pr2
# The URDF description of the robot must have
# been loaded in robot_description parameter
# on the Ros Parameter Server
# 1. Init robot, ros binding, solver
from dynamic_graph.sot.pr2.pr2_tasks import *
from dynamic_graph.sot.pr2.robot import *
from dynamic_graph.sot.core import RobotSimu
from dynamic_graph import plug
robot = Pr2('PR2', device=RobotSimu('PR2'))
plug(robot.device.state, robot.dynamic.position)
# 2. Ros binding
# roscore must be running
from dynamic_graph.ros import Ros
ros = Ros(robot)
# Use kine solver (with inequalities)
solver = initialize(robot)
# 4. Define a position task for the right hand
from dynamic_graph.sot.core.meta_tasks_kine import gotoNd, MetaTaskKine6d
from numpy import eye
from dynamic_graph.sot.core.matrix_util import matrixToTuple
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', 'right-wrist')
Pr2handMgrip = eye(4)
Pr2handMgrip[0:3, 3] = (0.18, 0, 0)
taskRH.opmodif = matrixToTuple(Pr2handMgrip)
signal = '{0}.{1}'.format(entityName, signalName)
filename = '{0}-{1}'.format(entityName, signalName)
trace.add(signal, filename)
if autoRecompute:
device.after.addSignal(signal)
I4 = (
(1., 0, 0, 0),
(0, 1., 0, 0),
(0, 0, 1., 0),
(0, 0, 0, 1.),
)
model = RosRobotModel('ur5_dynamic')
device = RobotSimu('ur5_device')
rospy.init_node('fake')
model.loadUrdf(
"file:///local/ngiftsun/devel/ros/catkin_ws/src/ur_description/urdf/ur5_robot.urdf"
)
dimension = model.getDimension()
device.resize(dimension)
plug(device.state, model.position)
# Set velocity and acceleration to 0
model.velocity.value = dimension * (0., )
model.acceleration.value = dimension * (0., )
# Create taks for the base
