plug(robot.device.state,robot.features['feature_jl'].state)
robotDim = len(robot.dynamic.velocity.value)
robot.features['feature_jl'].posture.value = robot.halfSitting
# Right Leg, Left leg, chest+neck, right arm, left arm
postureTaskDofs = \
[False]*3 + [False]*3 + [False]*3 + [False]*3 \
+ [True] *1 + [False]*3 \
+ [False]*1 + [True] *1 + [False]*5 + [False]*2 \
+ [False]*1 + [True] *1 + [False]*5 + [False]*2
for dof,isEnabled in enumerate(postureTaskDofs):
robot.features['feature_jl'].selectDof(dof+6,isEnabled)
taskJointLimit=TaskInequality('taskJL')
taskJointLimit.add('feature_jl')
# Limits on these joints: Shoulder Roll, Thumb, Fingers (Left and Right)
#taskJointLimit.referenceInf.value = (r_hip_yaw, r_hip_roll, r_hip_pitch, l_hip_yaw, l_hip_roll, l_hip_pitch, -1.55 ,1.57, 1.57,-0.005, -1.57,-1.57)
#taskJointLimit.referenceSup.value = (r_hip_yaw, r_hip_roll, r_hip_pitch, l_hip_yaw, l_hip_roll, l_hip_pitch, 0.005,1.57, 1.57, 1.55 , -1.57,-1.57)
taskJointLimit.referenceInf.value = (0.00,-1.550,-0.005)
taskJointLimit.referenceSup.value = (0.10, 0.005, 1.550)
taskJointLimit.dt.value= 0.005
taskJointLimit.controlGain.value = 0.009
#########################################################
bottle = createBottle()
estimateBottleFrameInHand(robot)
estimateCupFrameInHand(robot)
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)
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug, writeGraph
from dynamic_graph.sot.core.meta_task_6d import toFlags
from dynamic_graph.sot.dyninv import TaskInequality, TaskJointLimits
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d
from dynamic_graph.sot.dyninv import SolverKine
import dynamic_graph.sotcollision as sc
from numpy import *
# JOINT LIMIT INEQUALITY
joint_limit_task=TaskInequality('joint_inequality')
joint_feature = FeatureGeneric('joint_state_feature')
plug(robot.dynamic.position,joint_feature.errorIN)
joint_jacobian = matrixToTuple(eye(39))
joint_feature.jacobianIN.value = joint_jacobian
joint_limit_task.add(joint_feature.name)
joint_limit_task.referenceInf.value = (0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, -3.007, -0.471238, -0.714601836603, -0.5236, -0.8, -2.3213, -1.7976931348623157e+308, -2.18, -1.7976931348623157e+308, 0.0, 0.0, -0.1, -1.7976931348623157e+308, 0.0, 0.0, 0.0, -0.7854, -2.2853981634, -0.5236, -3.9, -2.3213, -1.7976931348623157e+308, -2.18, -1.7976931348623157e+308, 0.0, 0.0, -0.1, -1.7976931348623157e+308, 0.0, 0.0, 0.0, -1.7976931348623157e+308)
joint_limit_task.referenceSup.value = (1.0, 1.0, 0.0, 0, 0.0, 3.14, 0.33, 3.007, 1.39626, 2.2853981634, 1.3963, 3.9, 0.0, 1.7976931348623157e+308, 0.0, 1.7976931348623157e+308, 0.548, 0.548, 0.1, 1.7976931348623157e+308, 0.548, 0.548, 0.09, 1.48353, 0.714601836603, 1.3963, 0.8, 0.0, 1.7976931348623157e+308, 0.0, 1.7976931348623157e+308, 0.548, 0.548, 0.1, 1.7976931348623157e+308, 0.548, 0.548, 0.088, 1.7976931348623157e+308)
joint_limit_task.dt.value=1
joint_limit_task.controlGain.value=1.0
#WAIST TASK##
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 = '111101'#RzRyRxTzTyTx
task_waist_metakine.gain.setConstant(1)
task_waist_metakine.featureDes.position.value = goal_waist
##POSTURE TASK###
posture_feature = FeaturePosture('featurePosition')
#a.createcollisionpair("base_link","wall1")
#a.createcollisionpair("base_link","wall2")
#a.createcollisionpair("base_link","wall3")
a.createcollisionpair("base_link_1","platform1")
a.createcollisionpair("base_link_1","platform2")
a.createcollisionpair("base_link_2","platform1")
a.createcollisionpair("base_link_2","platform2")
a.createcollisionpair("base_link_3","platform1")
a.createcollisionpair("base_link_3","platform2")
a.createcollisionpair("base_link_4","platform1")
a.createcollisionpair("base_link_4","platform2")
task_collision_avoidance=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.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1)
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
#task_collision_avoidance.selec.value='110001'
task_waist_metakine=MetaTaskKine6d('task_waist_metakine',robot.dynamic,'base_joint','base_joint')
goal = ((1.,0,0,0.0),(0,1.,0,0.0),(0,0,1.,0),(0,0,0,1.),)
task_waist_metakine.gain.setConstant(8)
task_waist_metakine.featureDes.position.value = goal
a.collisionJacobian.recompute(0)
print a.collisionJacobian
a.collisionDistance.recompute(0)
print a.collisionDistance
#robot.traceDefaultSignals()
robot.addTrace(task_waist_metakine.task.name,'error')
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)
