class Solver:
def __init__(self, robot):
self.robot = robot
# Make sure control does not exceed joint limits.
self.jointLimitator = JointLimitator("joint_limitator")
plug(self.robot.dynamic.position, self.jointLimitator.joint)
plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)
# Create the solver.
self.sot = SolverKine("solver")
self.sot.signal("damping").value = 1e-6
self.sot.setSize(self.robot.dimension)
# Plug the solver control into the filter.
plug(self.sot.control, self.jointLimitator.controlIN)
# Important: always use 'jointLimitator.control'
# and NOT 'sot.control'!
if robot.device:
plug(self.jointLimitator.control, robot.device.control)
def push(self, task):
"""
Proxy method to push a task (not a MetaTask) in the sot
"""
self.sot.push(task.name)
if task.name != "taskposture" and "taskposture" in self.toList():
self.sot.down("taskposture")
def rm(self, task):
"""
Proxy method to remove a task from the sot
"""
self.sot.rm(task.name)
def pop(self):
"""
Proxy method to remove the last (usually posture) task from the sot
"""
self.sot.pop()
def __str__(self):
return self.sot.display()
def toList(self):
"""
Creates the list of the tasks in the sot
"""
return map(lambda x: x[1:-1], self.sot.dispStack().split("|")[1:])
def clear(self):
"""
Proxy method to remove all tasks from the sot
"""
self.sot.clear()
class Solver:
def __init__(self, robot):
self.robot = robot
# Make sure control does not exceed joint limits.
self.jointLimitator = JointLimitator('joint_limitator')
plug(self.robot.dynamic.position, self.jointLimitator.joint)
plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)
# Create the solver.
self.sot = SolverKine('solver')
self.sot.signal('damping').value = 1e-6
self.sot.setSize(self.robot.dimension)
# Plug the solver control into the filter.
plug(self.sot.control, self.jointLimitator.controlIN)
# Important: always use 'jointLimitator.control'
# and NOT 'sot.control'!
if robot.device:
plug(self.jointLimitator.control, robot.device.control)
def push(self, task):
"""
Proxy method to push a task (not a MetaTask) in the sot
"""
self.sot.push(task.name)
if task.name != "taskposture" and "taskposture" in self.toList():
self.sot.down("taskposture")
def rm(self, task):
"""
Proxy method to remove a task from the sot
"""
self.sot.rm(task.name)
def pop(self):
"""
Proxy method to remove the last (usually posture) task from the sot
"""
self.sot.pop()
def __str__(self):
return self.sot.display()
def toList(self):
"""
Creates the list of the tasks in the sot
"""
return map(lambda x: x[1:-1], self.sot.dispStack().split('|')[1:])
def clear(self):
"""
Proxy method to remove all tasks from the sot
"""
self.sot.clear()
class RobotCollisionModel:
def __init__(self):
# Marker array for link collision model
self.linkcollisionmodel = MarkerArray()
# Define marker properties for link
self.rcm_pub = rospy.Publisher("collision_model", MarkerArray)
self.r = rospy.Rate(1)
self.nolinks = 2
self.dt = 0
self.buildurrobot()
def buildurrobot(self):
# collision components
self.robot = Pr2(name="robot", device=RobotSimu("Pr2"))
self.dimension = self.robot.dynamic.getDimension()
plug(self.robot.device.state, self.robot.dynamic.position)
self.ros = Ros(self.robot)
self.a = sc.SotCollision("sc")
# create links for collision check
# self.a.createcollisionlink("wall1","box","external",(2.04,0.015,0.3,0,0,0.0,0.0,0,0))
self.a.createcollisionlink("base_link", "box", "internal", (0.65, 0.65, 0.25, 0.0, 0.0, 0.175, 0, 0, 0))
self.a.createcollisionlink(
"shoulder_right_link", "box", "internal", (0.20, 0.23, 0.65, 0.0, 0.0, -0.13, 0, 0, 0)
)
self.a.createcollisionlink(
"shoulder_left_link", "box", "internal", (0.20, 0.23, 0.65, 0.0, 0.0, -0.13, 0, 0, 0)
)
self.a.createcollisionlink("upper_left_arm", "box", "internal", (0.42, 0.13, 0.15, 0.26, 0.0, -0.0, 0, 0, 0))
self.a.createcollisionlink("upper_right_arm", "box", "internal", (0.42, 0.13, 0.15, 0.26, 0.0, -0.0, 0, 0, 0))
self.a.createcollisionlink(
"upper_left_forearm", "box", "internal", (0.37, 0.09, 0.09, 0.28, 0.0, -0.0, 0, 0, 0)
)
self.a.createcollisionlink(
"upper_right_forearm", "box", "internal", (0.37, 0.09, 0.09, 0.28, 0.0, -0.0, 0, 0, 0)
)
self.a.createcollisionlink("upper_right_wrist", "box", "internal", (0.1, 0.09, 0.05, 0.1, 0.0, -0.0, 0, 0, 0))
self.a.createcollisionlink("upper_left_wrist", "box", "internal", (0.1, 0.09, 0.05, 0.1, 0.0, -0.0, 0, 0, 0))
# self.a.createcollisionlink("platform1","box","external",(0.5,0.8,0.11,0.0,0.0,0.0,0,0.0,0))
# plug joint position and joint jacobian to collision checker entity
# self.a.wall1.value = ((1,0,0,0),(0,1,0,1.22),(0,0,1,0.12),(0,0,0,1))
# self.a.platform1.value = ((1,0,0,12),(0,1,0,0.82),(0,0,1,0.025),(0,0,0,1))
# self.a.platform2.value = ((1,0,0,0.77),(0,1,0,-0.82),(0,0,1,0.025),(0,0,0,1))
# print self.robot.dynamic
plug(self.robot.dynamic.base_joint, self.a.base_link)
plug(self.robot.dynamic.r_shoulder_pan_joint, self.a.shoulder_right_link)
plug(self.robot.dynamic.l_shoulder_pan_joint, self.a.shoulder_left_link)
plug(self.robot.dynamic.l_upper_arm_roll_joint, self.a.upper_left_arm)
plug(self.robot.dynamic.r_upper_arm_roll_joint, self.a.upper_right_arm)
plug(self.robot.dynamic.l_forearm_roll_joint, self.a.upper_left_forearm)
plug(self.robot.dynamic.r_forearm_roll_joint, self.a.upper_right_forearm)
plug(self.robot.dynamic.r_wrist_roll_joint, self.a.upper_right_wrist)
plug(self.robot.dynamic.l_wrist_roll_joint, self.a.upper_left_wrist)
# plug(self.robot.dynamic.base_joint,self.a.base_link_2)
# plug(self.robot.dynamic.base_joint,self.a.base_link_3)
# plug(self.robot.dynamic.base_joint,self.a.base_link_4)
# l_shoulder_pan_joint
# l_shoulder_lift_joint
# r_shoulder_pan_joint
# r_shoulder_lift_joint
# self.a.Jwall1.value = ((1,0,0,0),(0,1,0,0),(0,0,1,0),(0,0,0,1))
# self.a.Jwall2.value = ((1,0,0,0),(0,1,0,0),(0,0,1,0),(0,0,0,1))
# self.a.Jwall3.value = ((1,0,0,0),(0,1,0,0),(0,0,1,0),(0,0,0,1))
# self.a.Jplatform1.value = ((1,0,0,0),(0,1,0,0),(0,0,1,0),(0,0,0,1))
# self.a.Jplatform2.value = ((1,0,0,0),(0,1,0,0),(0,0,1,0),(0,0,0,1))
plug(self.robot.dynamic.Jbase_joint, self.a.Jbase_link)
plug(self.robot.dynamic.Jr_shoulder_pan_joint, self.a.Jshoulder_right_link)
plug(self.robot.dynamic.Jl_shoulder_pan_joint, self.a.Jshoulder_left_link)
plug(self.robot.dynamic.Jl_upper_arm_roll_joint, self.a.Jupper_left_arm)
plug(self.robot.dynamic.Jr_upper_arm_roll_joint, self.a.Jupper_right_arm)
plug(self.robot.dynamic.Jl_forearm_roll_joint, self.a.Jupper_left_forearm)
plug(self.robot.dynamic.Jr_forearm_roll_joint, self.a.Jupper_right_forearm)
plug(self.robot.dynamic.Jr_wrist_roll_joint, self.a.Jupper_right_wrist)
plug(self.robot.dynamic.Jl_wrist_roll_joint, self.a.Jupper_left_wrist)
# plug(self.robot.dynamic.Jbase_joint,self.a.Jbase_link_4)
self.a.createcollisionpair("base_link", "upper_left_arm")
self.a.createcollisionpair("base_link", "upper_right_arm")
self.a.createcollisionpair("base_link", "upper_left_forearm")
self.a.createcollisionpair("base_link", "upper_right_forearm")
self.a.createcollisionpair("shoulder_left_link", "upper_left_forearm")
self.a.createcollisionpair("shoulder_right_link", "upper_right_forearm")
self.a.createcollisionpair("upper_left_forearm", "upper_right_forearm")
self.a.createcollisionpair("upper_left_arm", "upper_right_arm")
self.a.createcollisionpair("upper_left_arm", "upper_right_forearm")
self.a.createcollisionpair("upper_left_forearm", "upper_right_arm")
self.a.createcollisionpair("upper_left_wrist", "upper_right_wrist")
self.a.createcollisionpair("upper_left_forearm", "upper_right_wrist")
self.a.createcollisionpair("upper_left_arm", "upper_right_wrist")
self.a.createcollisionpair("upper_right_wrist", "upper_left_wrist")
self.a.createcollisionpair("upper_right_forearm", "upper_left_wrist")
self.a.createcollisionpair("upper_right_arm", "upper_left_wrist")
# joint limits
self.robot.dynamic.upperJl.recompute(0)
self.robot.dynamic.lowerJl.recompute(0)
self.taskjl = TaskJointLimits("taskJL")
plug(self.robot.dynamic.position, self.taskjl.position)
self.taskjl.controlGain.value = 5
self.taskjl.referenceInf.value = self.robot.dynamic.lowerJl.value
# print self.robot.dynamic.lowerJl.value
self.taskjl.referenceSup.value = self.robot.dynamic.upperJl.value
# print self.robot.dynamic.upperJl.value
self.taskjl.dt.value = 1
# torsotask
self.task_torso_metakine = MetaTaskKine6d(
"task_torso_metakine", self.robot.dynamic, "torso_lift_joint", "torso_lift_joint"
)
goal_torso = ((1.0, 0, 0, -0.05), (0, 1.0, 0, -0.0), (0, 0, 1.0, 0.790675), (0, 0, 0, 1.0))
self.task_torso_metakine.feature.frame("desired")
# task_torso_metakine.feature.selec.value = '000100'#RzRyRxTzTyTx
self.task_torso_metakine.gain.setConstant(10)
self.task_torso_metakine.featureDes.position.value = goal_torso
goal = ((1.0, 0, 0, 0.0), (0, 1.0, 0, 0), (0, 0, 1.0, 0), (0, 0, 0, 1.0))
self.task_waist_metakine = MetaTaskKine6d("task_waist_metakine", self.robot.dynamic, "base_joint", "base_joint")
# task_waist_metakine.opmodif = goal
# task_waist_metakine.feature.frame('desired')
self.task_waist_metakine.featureDes.position.value = goal
self.task_waist_metakine.gain.setConstant(1)
self.task_l_wrist_metakine = MetaTaskKine6d(
"task_l_wrist_metakine", self.robot.dynamic, "l_wrist_roll_joint", "l_wrist_roll_joint"
)
# self.goal_l_wrist = ((1.,0,0,0.748),(0,1.,0,-0.246),(0,0,1.,0.639),(0,0,0,1.),)
self.goal_l_wrist = ((1.0, 0, 0, 0.649), (0, 1.0, 0, -0.042), (0, 0, 1.0, 0.845), (0, 0, 0, 1.0))
# self.goal_l_wrist = ((1.,0,0,0.486),(0,1.,0,-0.251),(0,0,1.,0.826),(0,0,0,1.),)
self.task_l_wrist_metakine.feature.frame("desired")
self.task_l_wrist_metakine.feature.selec.value = "000111" # RzRyRxTzTyTx
self.task_l_wrist_metakine.gain.setConstant(6)
self.task_l_wrist_metakine.featureDes.position.value = self.goal_l_wrist
self.task_r_wrist_metakine = MetaTaskKine6d(
"task_r_wrist_metakine", self.robot.dynamic, "r_wrist_roll_joint", "r_wrist_roll_joint"
)
# self.goal_l_wrist = ((1.,0,0,0.748),(0,1.,0,-0.246),(0,0,1.,0.639),(0,0,0,1.),)
# self.goal_r_wrist = ((1.,0,0,0.455),(0,1.,0,-0.835),(0,0,1.,0.780),(0,0,0,1.),)
self.goal_r_wrist = ((1.0, 0, 0, 0.590), (0, 1.0, 0, -0.188), (0, 0, 1.0, 1.107), (0, 0, 0, 1.0))
self.task_r_wrist_metakine.feature.frame("desired")
self.task_r_wrist_metakine.feature.selec.value = "000100" # RzRyRxTzTyTx
self.task_r_wrist_metakine.gain.setConstant(6)
self.task_r_wrist_metakine.featureDes.position.value = self.goal_r_wrist
self.task_collision_avoidance = TaskInequality("taskcollision")
self.collision_feature = FeatureGeneric("collisionfeature")
plug(self.a.collisionJacobian, self.collision_feature.jacobianIN)
plug(self.a.collisionDistance, self.collision_feature.errorIN)
self.task_collision_avoidance.add(self.collision_feature.name)
self.task_collision_avoidance.referenceInf.value = (
0.1,
0.1,
0.1,
0.1,
0.1,
0.1,
0.1,
0.1,
0.1,
0.1,
0.1,
0.1,
0.1,
0.1,
0.1,
0.1,
)
self.task_collision_avoidance.referenceSup.value = (
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
)
self.task_collision_avoidance.dt.value = 1
self.task_collision_avoidance.controlGain.value = 50.0
self.solver = SolverKine("sot")
self.solver.setSize(self.dimension)
self.solver.damping.value = 3e-2
self.solver.push(self.taskjl.name)
self.solver.push(self.task_collision_avoidance.name)
self.solver.push(self.task_waist_metakine.task.name)
self.solver.push(self.task_torso_metakine.task.name)
self.solver.push(self.task_r_wrist_metakine.task.name)
self.solver.push(self.task_l_wrist_metakine.task.name)
# self.solver.push (self.task_collision_avoidance.name)
# self.solver.push (self.task_wrist_metakine.task.name)
# self.solver.damping.value =3e-2
plug(self.solver.control, self.robot.device.control)
def publish(self):
# self.robot.dynamic.Jwrist_2_joint.recompute(self.dt)
# self.a.collisionDistance.recompute(self.dt)
# print self.a.collisionDistance.value
# self.a.collisionJacobian.recompute(self.dt)
# print self.a.collisionDistance.value
self.dt = self.dt + 1
# self.r.sleep()
# print self.robot.dynamic.base_joint.value
tstart = dt.datetime.now()
self.robot.device.increment(0.01)
self.getstate()
self.rcm_pub.publish(self.linkcollisionmodel)
tend = dt.datetime.now()
print(tend - tstart).microseconds
self.robot.dynamic.l_upper_arm_roll_joint.recompute(self.dt)
# print self.robot.dynamic.l_upper_arm_roll_joint.value
def getstate(self):
state = rcm.a.getfclstate()
# print state
# a = self.robot.dynamic.wrist_2_joint.value
for i in range(9):
self.link = Marker()
self.link.type = Marker().CUBE
# self.link.mesh_resource = "package://sot-collision/mesh/capsule/capsule.dae"
self.link.header.frame_id = "/odom"
self.link.header.stamp = rospy.Time.now()
# self.link.ns = "basicshapes"
self.link.id = i
self.link.action = self.link.ADD
# print state
self.link.pose.position.x = state[i][0]
self.link.pose.position.y = state[i][1]
self.link.pose.position.z = state[i][2]
self.link.pose.orientation.x = state[i][3]
self.link.pose.orientation.y = state[i][4]
self.link.pose.orientation.z = state[i][5]
self.link.pose.orientation.w = state[i][6]
self.link.scale.x = state[i][7]
self.link.scale.y = state[i][8]
self.link.scale.z = state[i][9]
self.link.color.r = 0.0
self.link.color.g = 1.0
self.link.color.b = 0.0
self.link.color.a = 1.0
# pself.link.pose.orientation
# self.link.lifetime = rospy.Duration();
self.linkcollisionmodel.markers.append(self.link)
# --- TASK RIGHT FOOT
# Task right hand
taskRF = MetaTask6d('rf', dyn, 'rf', 'right-ankle')
taskLF = MetaTask6d('lf', dyn, 'lf', 'left-ankle')
plug(pg.rightfootref, taskRF.featureDes.position)
taskRF.task.controlGain.value = 5
plug(pg.leftfootref, taskLF.featureDes.position)
taskLF.task.controlGain.value = 5
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
sot = SolverKine('sot')
sot.setSize(robotDim)
sot.push(taskWaist.task.name)
sot.push(taskRF.task.name)
sot.push(taskLF.task.name)
sot.push(taskComPD.name)
plug(sot.control, robot.control)
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.parseCmd(':SetAlgoForZmpTrajectory Herdt')
pg.parseCmd(':doublesupporttime 0.1')
pg.parseCmd(':singlesupporttime 0.7')
# When velocity reference is at zero, the robot stops all motion after n steps
pg.parseCmd(':numberstepsbeforestop 4')
# Set constraints on XY
pg.parseCmd(':setfeetconstraint XY 0.09 0.06')
tr.add('featureComDes.errorIN', 'comref')
tr.add('dyn.com', 'com')
tr.add(taskWaist.gain.name + '.gain', 'gainwaist')
history = History(dyn, 1)
# --- RUN -----------------------------------------------------------------
featurePosture.selec.value = toFlags(range(6, 36))
sot.clear()
for task in [taskWaist, taskRF, taskLF]:
task.feature.position.recompute(0)
task.feature.keep()
task.feature.selec.value = '111111'
sot.push(task.task.name)
taskWaist.ref = matrixToTuple(eye(4))
taskWaist.feature.selec.value = '111011'
taskWaist.gain.setByPoint(18, 0.1, 0.005, 0.8)
attime(200, (lambda: sot.rm(taskWaist.task.name), 'Rm waist'),
(lambda: pg.initState(), 'Init PG'),
(lambda: pg.parseCmd(":stepseq " + seqpart), 'Push step list'),
(lambda: sot.push(taskCom.name), 'Push COM'),
(lambda: plug(pg.rightfootref, taskRF.featureDes.position), 'Plug RF'),
(lambda: plug(pg.leftfootref, taskLF.featureDes.position), 'plug LF'))
#attime(700,lambda: sot.push(taskPosture.name),'Add Posture')
#attime(900,lambda: sot.rm(taskCom.name),'rm com')
#attime(1200,lambda: dump(),'dump!')
# --- TASK RIGHT FOOT
# Task right hand
taskRF=MetaTask6d('rf',dyn,'rf','right-ankle')
taskLF=MetaTask6d('lf',dyn,'lf','left-ankle')
plug(pg.rightfootref,taskRF.featureDes.position)
taskRF.task.controlGain.value = 5
plug(pg.leftfootref,taskLF.featureDes.position)
taskLF.task.controlGain.value = 5
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
sot = SolverKine('sot')
sot.setSize(robotDim)
sot.push(taskWaist.task.name)
sot.push(taskRF.task.name)
sot.push(taskLF.task.name)
sot.push(taskComPD.name)
plug(sot.control,robot.control)
# --- TRACER -----------------------------------------------------------------
from dynamic_graph.tracer import *
from dynamic_graph.tracer_real_time import *
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
tr.start()
robot.after.addSignal('tr.triger')
#tr.add(dyn.name+'.ffposition','ff')
taskPosition = Task('taskPosition')
taskPosition.add('featurePosition')
# featurePosition.selec.value = toFlags((6,24))
gainPosition = GainAdaptive('gainPosition')
gainPosition.set(0.1,0.1,125e3)
gainPosition.gain.value = 5
plug(taskPosition.error,gainPosition.error)
plug(gainPosition.gain,taskPosition.controlGain)
featurePosition.selec.value = '000000000000001111111111111100000000000'
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
sot.push(taskWaist.task.name)
sot.push(taskRF.task.name)
sot.push(taskLF.task.name)
sot.push(taskCom.task.name)
sot.push(taskWaistOr.task.name)
# Stun the upper part of the body.
sot.push(taskHead.task.name) # constraint the head orientation: look straight ahead
sot.push('taskPosition') # stun the arms.
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.startHerdt(False)
# You can now modifiy the speed of the robot using set pg.pg.velocitydes [3]( x, y, yaw)
class RobotCollisionModel:
def __init__(self):
# Marker array for link collision model
self.linkcollisionmodel = MarkerArray()
# Define marker properties for link
self.rcm_pub = rospy.Publisher("collision_model", MarkerArray)
self.r = rospy.Rate(100)
self.nolinks = 6
self.dt = 0
self.buildurrobot()
def buildurrobot(self):
self.robot = Ur("ur5", device=RobotSimu("ur5"))
robot_dimension = self.robot.dynamic.getDimension()
self.robot.device.resize(robot_dimension)
# Ros binding
# roscore must be running
ros = Ros(self.robot)
# collision components
self.a = sc.SotCollision("sc")
# self.a.createlinkmodel(((0,0.08,0.01,0,0,0.0,0.0,0,0),(1,0.08,0.04,0,0,0,0,0,0),(2,0.09,0.15,0.2,0.0,0.0,0,1.57,0),(3,0.07,0.14,0.2,0.0,0.0,-3.1416,1.5706,-3.1416),(4,0.05,0.03,0.0,0.093,0,-3.14,0,-3.14),(5,0.057,0.02,0.0,0,-0.095,-3.14,0,-3.14),(6,0.04,0.01,0.0,0.065,0,1.57,0,0)))
# create links for collision check
self.a.createcollisionlink("base_link", "capsule", "internal", (0.08, 0.01, 0, 0, 0, 0.0, 0.0, 0, 0))
self.a.createcollisionlink("shoulder_pan_link", "capsule", "internal", (0.08, 0.04, 0, 0, 0, 0, 0, 0, 0))
self.a.createcollisionlink(
"shoulder_lift_link", "capsule", "internal", (0.09, 0.15, 0, 0.2, 0.0, 0.0, 0, 1.57, 0)
)
self.a.createcollisionlink(
"elbow_link", "capsule", "internal", (0.07, 0.14, 0, 0.2, 0.0, 0.0, -3.1416, 1.5706, -3.1416)
)
self.a.createcollisionlink(
"wrist_1_link", "capsule", "internal", (0.05, 0.03, 0, 0.0, 0.093, 0, -3.14, 0, -3.14)
)
self.a.createcollisionlink(
"wrist_2_link", "capsule", "internal", (0.057, 0.02, 0, 0.0, 0, -0.095, -3.14, 0, -3.14)
)
self.a.createcollisionlink("wrist_3_link", "capsule", "internal", (0.04, 0.01, 0, 0.0, 0.065, 0, 1.57, 0, 0))
# plug joint position and joint jacobian to collision checker entity
plug(self.robot.dynamic.base_joint, self.a.base_link)
plug(self.robot.dynamic.shoulder_pan_joint, self.a.shoulder_pan_link)
plug(self.robot.dynamic.shoulder_lift_joint, self.a.shoulder_lift_link)
plug(self.robot.dynamic.elbow_joint, self.a.elbow_link)
plug(self.robot.dynamic.wrist_1_joint, self.a.wrist_1_link)
plug(self.robot.dynamic.wrist_2_joint, self.a.wrist_2_link)
plug(self.robot.dynamic.wrist_3_joint, self.a.wrist_3_link)
plug(self.robot.dynamic.Jbase_joint, self.a.Jbase_link)
plug(self.robot.dynamic.Jshoulder_pan_joint, self.a.Jshoulder_pan_link)
plug(self.robot.dynamic.Jshoulder_lift_joint, self.a.Jshoulder_lift_link)
plug(self.robot.dynamic.Jelbow_joint, self.a.Jelbow_link)
plug(self.robot.dynamic.Jwrist_1_joint, self.a.Jwrist_1_link)
plug(self.robot.dynamic.Jwrist_2_joint, self.a.Jwrist_2_link)
plug(self.robot.dynamic.Jwrist_3_joint, self.a.Jwrist_3_link)
# create collision pairs
# base link
self.a.createcollisionpair("base_link", "shoulder_lift_link")
self.a.createcollisionpair("base_link", "elbow_link")
self.a.createcollisionpair("base_link", "wrist_1_link")
self.a.createcollisionpair("base_link", "wrist_2_link")
self.a.createcollisionpair("base_link", "wrist_3_link")
# shoulder pan link
self.a.createcollisionpair("shoulder_pan_link", "wrist_1_link")
self.a.createcollisionpair("shoulder_pan_link", "wrist_2_link")
self.a.createcollisionpair("shoulder_pan_link", "wrist_3_link")
self.a.createcollisionpair("shoulder_pan_link", "elbow_link")
# shoulder lift link
self.a.createcollisionpair("shoulder_lift_link", "wrist_1_link")
self.a.createcollisionpair("shoulder_lift_link", "wrist_2_link")
self.a.createcollisionpair("shoulder_lift_link", "wrist_3_link")
# shoulder elbow link
self.a.createcollisionpair("elbow_link", "wrist_2_link")
self.a.createcollisionpair("elbow_link", "wrist_3_link")
# shoulder wrist1 link
self.a.createcollisionpair("wrist_1_link", "wrist_3_link")
######### task description for old type of solver############
# Create task for the waist
robot_pose = ((1.0, 0, 0, 0), (0, 1.0, 0, 0), (0, 0, 1.0, 0), (0, 0, 0, 1.0))
feature_base = FeaturePosition(
"position_base", self.robot.dynamic.base_joint, self.robot.dynamic.Jbase_joint, robot_pose
)
task_base = Task("waist_task")
task_base.controlGain.value = 100.0
task_base.add(feature_base.name)
# Create task for the wrist3
I4 = ((1.0, 0, 0, 0.321), (0, 1.0, 0, 0.109), (0, 0, 1.0, 0.848), (0, 0, 0, 1.0))
feature_wrist = FeaturePosition(
"position_wrist", self.robot.dynamic.wrist_3_joint, self.robot.dynamic.Jwrist_3_joint, I4
)
task_wrist = Task("wrist_task")
task_wrist.controlGain.value = 1
task_wrist.add(feature_wrist.name)
######### task description for new type of solver############
# waist position task
goal = ((1.0, 0, 0, 0), (0, 1.0, 0, 0), (0, 0, 1.0, 0), (0, 0, 0, 1.0))
self.task_waist_metakine = MetaTaskKine6d("task_waist_metakine", self.robot.dynamic, "base_joint", "base_joint")
# task_waist_metakine.opmodif = goal
# task_waist_metakine.feature.frame('desired')
self.task_waist_metakine.featureDes.position.value = goal
self.task_waist_metakine.gain.setConstant(100)
# wrist_3 task
goal = ((1.0, 0, 0, 0.321), (0, 1.0, 0, 0.109), (0, 0, 1.0, 0.848), (0, 0, 0, 1.0))
self.task_wrist_metakine = MetaTaskKine6d(
"task_wrist_metakine", self.robot.dynamic, "wrist_3_joint", "wrist_3_joint"
)
# task_wrist_metakine.opmodif = goal
# task_wrist_metakine.feature.frame('desired')
self.task_wrist_metakine.featureDes.position.value = goal
self.task_wrist_metakine.gain.setConstant(1)
######### task description for collision task ############
self.task_collision_avoidance = TaskInequality("taskcollision")
self.collision_feature = FeatureGeneric("collisionfeature")
plug(self.a.collisionJacobian, self.collision_feature.jacobianIN)
plug(self.a.collisionDistance, self.collision_feature.errorIN)
# self.a.collisionDistance
self.task_collision_avoidance.add(self.collision_feature.name)
self.task_collision_avoidance.referenceInf.value = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0) # min
self.task_collision_avoidance.referenceSup.value = (
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
) # max
self.task_collision_avoidance.dt.value = self.dt
self.task_collision_avoidance.controlGain.value = 0.01
# task.featureDes.xy.value = (0,0)
"""
#task_wrist_metakine.opmodif = goal
#task_wrist_metakine.feature.frame('desired')
task_wrist_metakine.featureDes.position.value = goal
task_wrist_metakine.gain.setConstant(1)
"""
######### solver###########################################################
##### solver old############
"""
self.solver = SOT ('solver')
self.solver.setSize (robot_dimension)
self.solver.push (task_base.name)
self.solver.push (task_wrist.name)
plug (self.solver.control, self.robot.device.control)
"""
self.solver = SolverKine("sot")
self.solver.setSize(robot_dimension)
self.solver.push(self.task_waist_metakine.task.name)
self.solver.push(self.task_collision_avoidance.name)
self.solver.push(self.task_wrist_metakine.task.name)
self.solver.damping.value = 3e-2
plug(self.solver.control, self.robot.device.control)
def publish(self):
self.a.collisionJacobian.recompute(self.dt)
self.a.collisionDistance.recompute(self.dt)
print "colljac"
print self.a.collisionJacobian.value
print "colldist"
print self.a.collisionDistance.value
self.robot.device.increment(0.01)
"""
rcm.robot.device.set((8E-58,8E-58,8E-58,8E-58,8E-58,8E-58,8E-58,8E-58,8E-58,8E-58,,8E-58,8E-58))
rcm.robot.device.increment(0.01)
rcm.robot.dynamic.shoulder_pan_joint.recompute(dt)
rcm.robot.dynamic.shoulder_lift_joint.recompute(dt)
rcm.robot.dynamic.elbow_joint.recompute(dt)
rcm.robot.dynamic.wrist_1_joint.recompute(dt)
rcm.robot.dynamic.wrist_2_joint.recompute(dt)
rcm.robot.dynamic.wrist_3_joint.recompute(dt)
rcm.robot.dynamic.Jshoulder_pan_joint.recompute(dt)
rcm.robot.dynamic.Jshoulder_lift_joint.recompute(dt)
rcm.robot.dynamic.Jelbow_joint.recompute(dt)
rcm.robot.dynamic.Jwrist_1_joint.recompute(dt)
rcm.robot.dynamic.Jwrist_2_joint.recompute(dt)
rcm.a.collisionJacobian.recompute(dt)
print rcm.a.collisionJpair1.value
dt = dt +1
"""
self.robot.dynamic.shoulder_pan_joint.recompute(self.dt)
self.robot.dynamic.shoulder_lift_joint.recompute(self.dt)
self.robot.dynamic.elbow_joint.recompute(self.dt)
self.robot.dynamic.wrist_1_joint.recompute(self.dt)
self.robot.dynamic.wrist_2_joint.recompute(self.dt)
self.robot.dynamic.wrist_3_joint.recompute(self.dt)
self.robot.dynamic.Jshoulder_pan_joint.recompute(self.dt)
self.robot.dynamic.Jshoulder_lift_joint.recompute(self.dt)
self.robot.dynamic.Jelbow_joint.recompute(self.dt)
self.robot.dynamic.Jwrist_1_joint.recompute(self.dt)
self.robot.dynamic.Jwrist_2_joint.recompute(self.dt)
# recompute pair
# self.a.collisionJacobian.recompute(self.dt)
# self.a.imdpair1.recompute(self.dt)
# print self.a.imdpair1.value
# print 'im here'
self.dt = self.dt + 1
# self.a.updatefclmodel(1)
self.getstate()
# print 'wrist_3'
# print self.robot.dynamic.wrist_3_joint.value
self.rcm_pub.publish(self.linkcollisionmodel)
self.r.sleep()
# self.linkcollisionmodel = MarkerArray()
# 0.07 0.07 0.05
#
def getstate(self):
state = rcm.a.getfclstate()
# print state
# a = self.robot.dynamic.wrist_2_joint.value
for i in range(7):
self.link = Marker()
self.link.type = self.link.MESH_RESOURCE # Cylinder
self.link.mesh_resource = "package://sot-collision/mesh/capsule/capsule.dae"
self.link.header.frame_id = "/odom"
self.link.header.stamp = rospy.Time.now()
# self.link.ns = "basicshapes"
self.link.id = i
self.link.action = self.link.ADD
# print state
self.link.pose.position.x = state[i][0]
self.link.pose.position.y = state[i][1]
self.link.pose.position.z = state[i][2]
self.link.pose.orientation.x = state[i][3]
self.link.pose.orientation.y = state[i][4]
self.link.pose.orientation.z = state[i][5]
self.link.pose.orientation.w = state[i][6]
self.link.scale.x = state[i][7]
self.link.scale.y = state[i][7]
self.link.scale.z = state[i][8]
self.link.color.r = 0.0
self.link.color.g = 1.0
self.link.color.b = 0.0
self.link.color.a = 1.0
# pself.link.pose.orientation
# self.link.lifetime = rospy.Duration();
self.linkcollisionmodel.markers.append(self.link)
#taskCom0.task.controlGain.value = 10
#sot.push(taskCom0.task.name)
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.startHerdt()
# You can now modifiy the speed of the robot using set pg.pg.velocitydes [3]( x, y, yaw)
pg.pg.velocitydes.value = (0.1, 0.0, 0.0)
#ball0 = (2.5,-0.3,0.5)
ball0 = (0.5, -2.3, 0.5)
w = (1 / 1.3, 1 / 1.8, 1 / 3.9)
h = (0.1, 0.07, 0.3)
sot.damping.value = 5e-2
sot.push(taskJL.name)
sot.push(taskWaist.task.name)
sot.push(taskRF.task.name)
sot.push(taskLF.task.name)
sot.push(taskCom.task.name)
taskPosture.gotoq((5, 1, 0.05, 0.9), rhand=[
1,
])
sot.push(taskPosture.task.name)
gotoNd(taskGaze, ball0, '011', (10, 1, 0.01, 0.9))
sot.push(taskGaze.task.name)
gotoNd(taskRH, ball0, '011', (10, 1, 0.01, 0.9))
sot.push(taskRH.task.name)
# --- SCRIPT EVENTS ----------------------------------------------------------
#from random import random
#taskCom0.task.controlGain.value = 10 #sot.push(taskCom0.task.name) # --- HERDT PG AND START ------------------------------------------------------- # Set the algorithm generating the ZMP reference trajectory to Herdt's one. pg.startHerdt() # You can now modifiy the speed of the robot using set pg.pg.velocitydes [3]( x, y, yaw) pg.pg.velocitydes.value =(0.1,0.0,0.0) #ball0 = (2.5,-0.3,0.5) ball0 = (0.5,-2.3,0.5) w=(1/1.3,1/1.8,1/3.9) h=(0.1,0.07,0.3) sot.damping.value = 5e-2 sot.push(taskJL.name) sot.push(taskWaist.task.name) sot.push(taskRF.task.name) sot.push(taskLF.task.name) sot.push(taskCom.task.name) taskPosture.gotoq((5,1,0.05,0.9),rhand=[1,]) sot.push(taskPosture.task.name) gotoNd(taskGaze,ball0,'011',(10,1,0.01,0.9)) sot.push(taskGaze.task.name) gotoNd(taskRH,ball0,'011',(10,1,0.01,0.9)) sot.push(taskRH.task.name) # --- SCRIPT EVENTS ---------------------------------------------------------- #from random import random import numpy.random as nprand
robot.after.addSignal(taskJL.name+".normalizedPosition")
robot.after.addSignal(taskSupportSmall.name+".normalizedPosition")
robot.after.addSignal(taskFoV.task.name+".normalizedPosition")
robot.after.addSignal(taskFoV.feature.name+'.error')
robot.after.addSignal(taskSupport.name+".normalizedPosition")
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
taskCom.featureDes.errorIN.value = dyn.com.value
taskCom.task.controlGain.value = 10
taskPosture.gotoq(1,array(q0),chest=[],head=[],rleg=[],lleg=[],rarm=[],larm=[])
sot.push(taskJL.name)
sot.addContact(contactRF)
sot.addContact(contactLF)
sot.push(taskSupport.name)
ball = BallPosition((0,-1.1,0.9))
tw=TaskWeight('tw')
'''
taskFoV.task.controlGain.value=0.05
taskSupportSmall.controlGain.value=0.02
tw.add(taskSupportSmall.name,1)
tw.add(taskFoV.task.name,10)
sot.damping.value = 0.01
'''
tr.add('featureComDes.errorIN','comref')
tr.add('dyn.com','com')
tr.add(taskWaist.gain.name+'.gain','gainwaist')
history = History(dyn,1)
# --- RUN -----------------------------------------------------------------
featurePosture.selec.value = toFlags( range(6,36) )
sot.clear()
for task in [taskWaist, taskRF, taskLF]:
task.feature.position.recompute(0)
task.feature.keep()
task.feature.selec.value = '111111'
sot.push(task.task.name)
taskWaist.ref = matrixToTuple(eye(4))
taskWaist.feature.selec.value = '111011'
taskWaist.gain.setByPoint(18,0.1,0.005,0.8)
attime(200
,(lambda : sot.rm(taskWaist.task.name),'Rm waist')
,(lambda : pg.initState(),'Init PG')
,(lambda : pg.parseCmd(":stepseq " + seqpart),'Push step list')
,(lambda : sot.push(taskCom.name),'Push COM')
,(lambda : plug(pg.rightfootref,taskRF.featureDes.position),'Plug RF')
,(lambda : plug(pg.leftfootref,taskLF.featureDes.position),'plug LF' )
)
taskComPD.setBeta(-1)
# --- CONSTRAINT SUPPORT FOOT
featureSupport = FeatureGeneric('featureSupport')
featureSupport.errorIN.value = 6*(0,)
plug(footSelection.Jfoot,featureSupport.jacobianIN)
constraintSupport = Task('constraintSupport')
constraintSupport.add(featureSupport.name)
constraintSupport.task.value = 6*(0,)
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
sot = SolverKine('sot')
sot.setSize(robotDim)
sot.push(constraintSupport.name)
sot.push(taskTwoFeet.name)
sot.push(taskWaist.task.name)
sot.push(taskComPD.name)
plug(sot.control,robot.control)
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.parseCmd(':SetAlgoForZmpTrajectory Herdt')
pg.parseCmd(':doublesupporttime 0.1')
pg.parseCmd(':singlesupporttime 0.7')
# When velocity reference is at zero, the robot stops all motion after n steps
pg.parseCmd(':numberstepsbeforestop 4')
# Set constraints on XY
pg.parseCmd(':setfeetconstraint XY 0.09 0.06')
ip = (0.200, -0.006, 0.377,-0.155, 0.414, -0.555)
zp = (0.198, 0.001, 0.581 ,-0.000, 0.001, 0.010)
prl = (0.606, -0.001, 0.425,-3.115, 0.228, -3.029)
aov = (-0.136, -0.005, 0.389,0.005, -0.575, 0.010)
pp = (0.567, -0.013, 0.260,0,0,0)
pr = (0.008,0.04,0.515,0,0,0)
goal = matrixToTuple(generic6dReference(zp))
print goal
#solver.damping.value =3e-2
task_wrist_metakine.gain.setConstant(1)
task_wrist_metakine.feature.selec.value = '000111'
task_wrist_metakine.featureDes.position.value = goal
#solver
#solver.push (task_waist.name)
solver.push (taskjl.name)
solver.push (task_waist_metakine.task.name)
solver.push (task_wrist_metakine.task.name)
plug (solver.control,robot.device.control)
dt = 0.01
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
robot.device.increment(dt)
runner=inc()
runner.once()
[go,stop,next,n]=loopShortcuts(runner)
taskComPD.setBeta(-1)
# --- CONSTRAINT SUPPORT FOOT
featureSupport = FeatureGeneric('featureSupport')
featureSupport.errorIN.value = 6 * (0, )
plug(footSelection.Jfoot, featureSupport.jacobianIN)
constraintSupport = Task('constraintSupport')
constraintSupport.add(featureSupport.name)
constraintSupport.task.value = 6 * (0, )
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
sot = SolverKine('sot')
sot.setSize(robotDim)
sot.push(constraintSupport.name)
sot.push(taskTwoFeet.name)
sot.push(taskWaist.task.name)
sot.push(taskComPD.name)
plug(sot.control, robot.control)
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.parseCmd(':SetAlgoForZmpTrajectory Herdt')
pg.parseCmd(':doublesupporttime 0.1')
pg.parseCmd(':singlesupporttime 0.7')
# When velocity reference is at zero, the robot stops all motion after n steps
pg.parseCmd(':numberstepsbeforestop 4')
# Set constraints on XY
pg.parseCmd(':setfeetconstraint XY 0.09 0.06')
class Solver:
def __init__(self, robot):
self.robot = robot
"""
# Make sure control does not exceed joint limits.
self.jointLimitator = JointLimitator('joint_limitator')
plug(self.robot.dynamic.position, self.jointLimitator.joint)
plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)
"""
# Create the solver.
self.sot = SolverKine('solver')
self.sot.signal('damping').value = 1e-6
self.sot.setSize(self.robot.dimension)
# Set second order computation
self.robot.device.setSecondOrderIntegration()
self.sot.setSecondOrderKinematics()
plug(self.robot.device.velocity,self.robot.dynamic.velocity)
plug(self.robot.dynamic.velocity,self.sot.velocity)
# Plug the solver control into the robot.
plug(self.sot.control, robot.device.control)
def push (self, task):
"""
Proxy method to push a task (not a MetaTask) in the sot
"""
self.sot.push (task.name)
if task.name!="taskposture" and "taskposture" in self.toList():
self.sot.down("taskposture")
def rm (self, task):
"""
Proxy method to remove a task from the sot
"""
self.sot.rm (task.name)
def pop (self):
"""
Proxy method to remove the last (usually posture) task from the sot
"""
self.sot.pop ()
def __str__ (self):
return self.sot.display ()
def toList(self):
"""
Creates the list of the tasks in the sot
"""
return map(lambda x: x[1:-1],self.sot.dispStack().split('|')[1:])
def clear(self):
"""
Proxy method to remove all tasks from the sot
"""
self.sot.clear ()
taskPosition = Task('taskPosition')
taskPosition.add('featurePosition')
# featurePosition.selec.value = toFlags((6,24))
gainPosition = GainAdaptive('gainPosition')
gainPosition.set(0.1, 0.1, 125e3)
gainPosition.gain.value = 5
plug(taskPosition.error, gainPosition.error)
plug(gainPosition.gain, taskPosition.controlGain)
featurePosition.selec.value = '000000000000001111111111111100000000000'
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
sot.push(taskWaist.task.name)
sot.push(taskRF.task.name)
sot.push(taskLF.task.name)
sot.push(taskCom.task.name)
sot.push(taskWaistOr.task.name)
# Stun the upper part of the body.
sot.push(
taskHead.task.name) # constraint the head orientation: look straight ahead
sot.push('taskPosition') # stun the arms.
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.startHerdt(False)
# You can now modifiy the speed of the robot using set pg.pg.velocitydes [3]( x, y, yaw)
pg.pg.velocitydes.value = (0.1, 0.0, 0.0)
