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()
robot.dynamic.ffvelocity.unplug()
robot.dynamic.ffacceleration.unplug()
robot.dynamic.setProperty('ComputeBackwardDynamics','true')
robot.dynamic.setProperty('ComputeAccelerationCoM','true')
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
def toList(sot):
return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robot.dimension)
# The control signal, previously plugged to the velocity (kinematic mode) is
# redirected.
robot.device.control.unplug()
plug(sot.control,robot.device.control)
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASK GRIP ---
taskRH=MetaTaskKine6d('rh',robot.dynamic,'rh','right-wrist')
# change the position of the operational point wrt the wrist
handMgrip=eye(4); handMgrip[0:3,3] = (0.07,-0.02,0)
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 = 5
self.dt = 0
self.buildurrobot()
def buildurrobot(self):
# collision components
self.robot = youbot(name="robot", device=RobotSimu("youbot"))
self.dimension = self.robot.dynamic.getDimension()
plug(self.robot.device.state, self.robot.dynamic.position)
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("wall2", "box", "external", (0.015, -2.44, 0.3, 0, 0, 0, 0, 0, 0))
self.a.createcollisionlink("wall3", "box", "external", (2.04, 0.015, 0.3, 0, 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))
self.a.createcollisionlink("platform2", "box", "external", (0.5, 0.8, 0.11, 0.0, 0.0, 0.0, 0, 0.0, 0))
# self.a.createcollisionlink("base_link","box","internal",(0.64,0.4,0.11,0.0,0.0,0,0,0,0))
self.a.createcollisionlink("base_link_1", "box", "internal", (0.1, 0.4, 0.11, 0.26, 0.0, 0, 0, 0, 0))
self.a.createcollisionlink("base_link_2", "box", "internal", (0.1, 0.4, 0.11, -0.26, 0.0, 0, 0, 0, 0))
self.a.createcollisionlink("base_link_3", "box", "internal", (0.44, 0.1, 0.11, 0.0, 0.15, 0, 0, 0, 0))
self.a.createcollisionlink("base_link_4", "box", "internal", (0.44, 0.1, 0.11, 0.0, -0.15, 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.wall2.value = ((1, 0, 0, 1.02), (0, 1, 0, 0), (0, 0, 0, 0.12), (0, 0, 0, 1))
self.a.wall3.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, 0.77), (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))
# plug(self.robot.dynamic.base_joint,self.a.base_link)
plug(self.robot.dynamic.base_joint, self.a.base_link_1)
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)
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_1)
plug(self.robot.dynamic.Jbase_joint, self.a.Jbase_link_2)
plug(self.robot.dynamic.Jbase_joint, self.a.Jbase_link_3)
plug(self.robot.dynamic.Jbase_joint, self.a.Jbase_link_4)
self.a.createcollisionpair("base_link_1", "platform1")
self.a.createcollisionpair("base_link_1", "platform2")
self.a.createcollisionpair("base_link_2", "platform1")
self.a.createcollisionpair("base_link_2", "platform2")
self.a.createcollisionpair("base_link_3", "platform1")
self.a.createcollisionpair("base_link_3", "platform2")
self.a.createcollisionpair("base_link_4", "platform1")
self.a.createcollisionpair("base_link_4", "platform2")
goal = ((1.0, 0, 0, 0.5), (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.solver = SolverKine("sot")
self.solver.setSize(self.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.robot.dynamic.Jwrist_2_joint.recompute(self.dt)
self.a.collisionDistance.recompute(self.dt)
self.a.collisionJacobian.recompute(self.dt)
# print self.a.collisionDistance.value
self.dt = self.dt + 1
self.getstate()
self.rcm_pub.publish(self.linkcollisionmodel)
self.r.sleep()
self.robot.device.increment(0.01)
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)
dyn.setProperty('ComputeBackwardDynamics','true')
dyn.setProperty('ComputeAccelerationCoM','true')
robot.control.unplug()
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
def toList(sot):
return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robotDim)
plug(sot.control,robot.control)
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASK GRIP ---
taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist')
handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.21)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist')
taskLH.opmodif = matrixToTuple(handMgrip)
taskLH.feature.frame('desired')
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 ()
(0,0,1.,0.0),
(0,0,0,1.),)
feature_waist = FeaturePosition ('position_waist', robot.dynamic.base_joint,robot.dynamic.Jbase_joint, robot_pose)
task_waist = Task ('waist_task')
task_waist.controlGain.value = 0.5
task_waist.add (feature_waist.name)
#waisttask
task_waist_metakine=MetaTaskKine6d('task_waist_metakine',robot.dynamic,'base_joint','base_joint')
goal_waist = ((1.,0,0,0.0),(0,1.,0,-0.0),(0,0,1.,0.0),(0,0,0,1.),)
task_waist_metakine.feature.frame('desired')
#task_waist_metakine.feature.selec.value = '011101'#RzRyRxTzTyTx
task_waist_metakine.gain.setConstant(0.5)
task_waist_metakine.featureDes.position.value = goal_waist
solver = SolverKine('sot_solver')
solver.setSize (robot.dynamic.getDimension())
robot.device.resize (robot.dynamic.getDimension())
'''
#taskinequality
task_waist=TaskInequality('taskcollision')
collision_feature = FeatureGeneric('collisionfeature')
plug(a.collisionJacobian,collision_feature.jacobianIN)
plug(a.collisionDistance,collision_feature.errorIN)
task_collision_avoidance.add(collision_feature.name)
task_collision_avoidance.referenceInf.value = (0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05)
task_collision_avoidance.referenceSup.value = (2e10,2e10,2e10,2e10,2e10,2e10,2e10,2e10)
task_collision_avoidance.dt.value=1
task_collision_avoidance.controlGain.value=50.0
'''
# ll
ll = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.0100692,0.0100692,-5.02655,0.0221239,0.110619,0,0)
robot.control.unplug()
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
def toList(sot):
return map(lambda x: x[1:-1], sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robotDim)
plug(sot.control, robot.control)
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASK GRIP ---
taskRH = MetaTaskKine6d('rh', dyn, 'rh', 'right-wrist')
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.07, -0.02, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
taskLH = MetaTaskKine6d('lh', dyn, 'lh', 'left-wrist')
taskLH.opmodif = matrixToTuple(handMgrip)
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)
print("Number of dofs (6 for the freeflyer + num of joints):")
print robot.dimension
# FIXME: this must be set so that the graph can be evaluated.
robot.device.zmp.value = (0., 0., 0.)
# Create a solver.
from dynamic_graph.sot.dyninv import SolverKine
def toList(solver):
return map(lambda x: x[1:-1], solver.dispStack().split('|')[1:])
SolverKine.toList = toList
solver = SolverKine('sot')
solver.setSize(robot.dimension)
# set a constant damping to 0.1
solver.damping.value = 0.1
robot.device.control.unplug()
plug(solver.control, robot.device.control)
# Close the control loop
plug(robot.device.state, robot.dynamic.position)
print("...done!")
# Make sure only robot and solver are visible from the outside.
__all__ = ["robot", "solver"]
Device = PyEntityFactoryClass('SotDevice')
robot = RobotBuilder(name='robot', device=Device('robot_device'))
print("Number of dofs (6 for the freeflyer + num of joints):")
print robot.dimension
# FIXME: this must be set so that the graph can be evaluated.
robot.device.zmp.value = (0., 0., 0.)
# Create a solver.
from dynamic_graph.sot.dyninv import SolverKine
def toList(solver):
return map(lambda x: x[1:-1],solver.dispStack().split('|')[1:])
SolverKine.toList = toList
solver = SolverKine('sot')
solver.setSize(robot.dimension)
# set a constant damping to 0.1
solver.damping.value = 0.1
robot.device.control.unplug()
plug(solver.control,robot.device.control)
# Close the control loop
plug(robot.device.state,robot.dynamic.position)
print("...done!")
# Make sure only robot and solver are visible from the outside.
__all__ = ["robot", "solver"]
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()
robot.dynamic.ffvelocity.unplug()
robot.dynamic.ffacceleration.unplug()
robot.dynamic.setProperty('ComputeBackwardDynamics', 'true')
robot.dynamic.setProperty('ComputeAccelerationCoM', 'true')
robot.device.control.unplug()
plug(sot.control, robot.device.control)
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts
dt = 1e-3
robot.traceDefaultSignals()
############## new solver###############
dimension = robot.dynamic.getDimension()
solver = SolverKine('sot')
solver.setSize (dimension)
# create links for collision check
from dynamic_graph.sot.dyninv import SolverKine
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d
from dynamic_graph.sot.dyninv import TaskInequality, TaskJointLimits
import dynamic_graph.sotcollision as sc
#solver.damping.value =3e-2
a = sc.SotCollision("sc")
solver.damping.value =3
a.createcollisionlink("wall1","box","external",(2.04,0.015,0.3,0,0,0.0,0.0,0,0))
a.createcollisionlink("wall2","box","external",(0.015,-2.44,0.3,0,0,0,0,0,0))
a.createcollisionlink("wall3","box","external",(2.04,0.015,0.3,0,0,0.0,0.0,0,0))
a.createcollisionlink("platform1","box","external",(0.5,0.8,0.11,0.0,0.0,0.0,0,0.0,0))
def SotPr2(robot):
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robot.dimension)
return sot
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)
def SotPr2(robot):
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robot.dimension)
return sot
