def main():
init_pos = Vectors.V4D(0.65, 0.05, 0.03, 0)
bound = Vectors.V4D(0.65, 0.55, 0.53, 0)
edges = {
'init': init_pos,
'bound': bound
}
sock = create_socket()
try:
move.run(sock, 'left', left_arm, edges)
except KeyboardInterrupt:
sock.close()
def main():
init_pos = Vectors.V4D(0.656982770038, -0.752598021641, 0.0388609422173, 0)
bound = Vectors.V4D(0.656982770038, -0.252598021641, 0.5388609422173, 0)
edges = {
'init': init_pos,
'bound': bound
}
sock = create_socket()
try:
move.run(sock, 'right', right_arm, edges)
except KeyboardInterrupt:
sock.close()
def decide_increment(data):
#TODO - decide based on data
mutex.acquire()
global direction
global dz
global last_data
if last_data is None:
last_data = data
if (data is not None):
wow = data['onset']
dwow = data['onset'] - last_data['onset']
if abs(dwow) > 0.01:
dz = dwow / 10.0
print "dwow", dwow
print "dz: ", dz
inc = Vectors.V4D(0.0, direction * 0.05, dz, 0.0)
mutex.release()
last_data = data
return inc
def run(sock, arm, pose_func, edges):
current_p = edges['init']
i_bound = edges['init']
o_bound = edges['bound']
channel = Queue.Queue()
# initial values
increment = Vectors.V4D(0.00, 0.05, 0.00, 0);
speed = 0.5
# limit the amount of movement calls
lim = 1000
j = 0
mdata = None
while (j < lim):
j += 1
# socket call
try:
data, addr = sock.recvfrom(1024)
mdata = recv_data(data)
speed = mdata['energy']
def swap_inc(data):
while time.time() < data['beat']:
time.sleep(0.01)
global direction
mutex.acquire()
direction *= -1
mutex.release()
t = threading.Thread(target = swap_inc, args = (mdata,))
t.daemon = True
t.start()
except socket.error, e:
None
increment = decide_increment(mdata);
current_p = current_p + increment
current_p = clamp(current_p, i_bound, o_bound)
pose_func(current_p)
resp = ik_move(arm, pose_func(current_p))
if resp is not None:
make_move(resp, arm, speed)
print "--------"
time.sleep(0.1)
def left_arm(pos):
quaternion = Vectors.V4D(0.36, 0.88, -0.10, 0.26)
position = Pose(
position=Point(
x=pos.x(),
y=pos.y(),
z=pos.z(),
),
orientation=Quaternion(
x=quaternion.x(),
y=quaternion.y(),
z=quaternion.z(),
w=quaternion.w(),
),
)
return position
def clamp(p, inner, outer):
if p.x() > outer.x():
p = Vectors.V4D(outer.x(), p.y(), p.z(), p.w())
elif p.x() < inner.x():
p = Vectors.V4D(inner.x(), p.y(), p.z(), p.w())
if p.y() > outer.y():
p = Vectors.V4D(p.x(), outer.y(), p.z(), p.w())
elif p.y() < inner.y():
p = Vectors.V4D(p.x(), inner.y(), p.z(), p.w())
if p.z() > outer.z():
p = Vectors.V4D(p.x(), p.y(), outer.z(), p.w())
elif p.z() < inner.z():
p = Vectors.V4D(p.x(), p.y(), inner.z(), p.w())
if p.w() > outer.w():
p = Vectors.V4D(p.x(), p.y(), p.z(), outer.w())
elif p.w() < inner.w():
p = Vectors.V4D(p.x(), p.y(), p.z(), inner.w())
return p
def main():
userID = systemArgHandler()
global limbRight
global limbLeft
rospy.init_node('lift_ik_prototype', anonymous=True)
"""
myps = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=pose2(init_pos),
)
solve_move_trac(limbRight, myps)
"""
rate = rospy.Rate(5000.0)
#main loop
while not rospy.is_shutdown():
try:
#defines the pose of the child from the parent
buffUserLeft = tf_buffer.lookup_transform(
'cob_body_tracker/user_' + str(userID) + '/left_shoulder',
'cob_body_tracker/user_' + str(userID) + '/left_hand',
rospy.Time())
buffUserRight = tf_buffer.lookup_transform(
'cob_body_tracker/user_' + str(userID) + '/right_shoulder',
'cob_body_tracker/user_' + str(userID) + '/right_hand',
rospy.Time())
#get translations
xL = buffUserLeft.transform.translation.x
yL = buffUserLeft.transform.translation.y
zL = buffUserLeft.transform.translation.z
xR = buffUserRight.transform.translation.x
yR = buffUserRight.transform.translation.y
zR = buffUserRight.transform.translation.z
#translate the translations
#add offsets to each joint
tranXL = -zL * 1.2 + 0.2
tranYL = xL * 1.5 - 0.28
tranZL = -yL * 1.1 + 0.40
tranXR = -zR * 1.2 + 0.2
tranYR = xR * 1.5 + 0.28
tranZR = -yR * 1.1 + 0.40
#tranRightX = -z * 1.2
#tranRightY = x * 1.2
#tranRightZ = -y * 0.9
#catch and continue after refresh rate
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rate.sleep()
continue
user_pos_left = Vectors.V4D(
tranXL, #depth (z?) (inline depth 0.2) (limit 1.0)
tranYL, #left-right (x?) (inline 0.28) (limit 1.0)
tranZL,
0) #height (y?) (inline height 0.40)
user_pos_right = Vectors.V4D(
tranXR, #depth (z?) (inline depth 0.2) (limit 1.0)
tranYR, #left-right (x?) (inline 0.28) (limit 1.0)
tranZR,
0) #height (y?) (inline height 0.40)
#user_rot = Vectors.V4D(math.pi/2,
# ((math.atan2(y,x) + math.pi/2) % (math.pi * 2)) - 10*math.pi/180,
# 0, 0)
# rotation pointing the infront of Red
user_rot = Vectors.V4D(0, math.pi / 2, 0, 0)
rightArmPose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
#header=Header(stamp=rospy.Time.now(), frame_id=limbRight+'_arm_mount'),
#header=Header(stamp=rospy.Time.now(), frame_id=limbRight+'_lower_shoulder'),
pose=userPose(user_pos_left, user_rot),
)
leftArmPose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
#header=Header(stamp=rospy.Time.now(), frame_id=limbRight+'_arm_mount'),
#header=Header(stamp=rospy.Time.now(), frame_id=limbRight+'_lower_shoulder'),
pose=userPose(user_pos_right, user_rot),
)
#solve_move_trac(limbRight, leftArmPose)
solve_move_trac(limbLeft, leftArmPose)
#Refresh rate
rate.sleep()
#orientation=Quaternion(
# x=0,
# y=1,
# z=0,
# w=0
# ),
orientation=normalize(
Quaternion(x=q_rot[0], y=q_rot[1], z=q_rot[2], w=q_rot[3])))
return pose_right
#NOTE: right hand
#index finger point left
init_pos = Vectors.V4D(
0.6, #depth (z?) (inline depth 0.2) (limit 1.0)
-0.60, #left-right (x?) (inline 0.28) (limit 1.0)
0.40,
0) #height (y?) (inline height 0.40)
# Quaternion -> Quaternion
def normalize(quat):
quatNorm = math.sqrt(quat.x * quat.x + quat.y * quat.y + quat.z * quat.z +
quat.w * quat.w)
normQuat = Quaternion(quat.x / quatNorm, quat.y / quatNorm,
quat.z / quatNorm, quat.w / quatNorm)
return normQuat
def systemArgHandler():
if len(sys.argv) == 2:
def main():
userID = systemArgHandler()
global mylimb
rospy.init_node('lift_ik_prototype', anonymous=True)
"""
myps = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=pose2(init_pos),
)
solve_move_trac(mylimb, myps)
"""
rate = rospy.Rate(5000.0)
#main loop
while not rospy.is_shutdown():
try:
#defines the pose of the child from the parent
buffUserLeft = tf_buffer.lookup_transform(
'cob_body_tracker/user_' + str(userID) + '/left_shoulder',
'cob_body_tracker/user_' + str(userID) + '/left_hand',
rospy.Time())
#get translations
x = buffUserLeft.transform.translation.x
y = buffUserLeft.transform.translation.y
z = buffUserLeft.transform.translation.z
#translate the translations
tranRightX = -z * 1.2 + 0.2
tranRightY = x * 1.5 - 0.28
tranRightZ = -y * 1.1 + 0.40
#tranRightX = -z * 1.2
#tranRightY = x * 1.2
#tranRightZ = -y * 0.9
#catch and continue after refresh rate
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rate.sleep()
continue
user_pos = Vectors.V4D(
tranRightX, #depth (z?) (inline depth 0.2) (limit 1.0)
tranRightY, #left-right (x?) (inline 0.28) (limit 1.0)
tranRightZ,
0) #height (y?) (inline height 0.40)
#user_rot = Vectors.V4D(math.pi/2,
# ((math.atan2(y,x) + math.pi/2) % (math.pi * 2)) - 10*math.pi/180,
# 0, 0)
user_rot = Vectors.V4D(0, math.pi / 2, 0, 0)
leftArmPose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
#header=Header(stamp=rospy.Time.now(), frame_id=mylimb+'_arm_mount'),
#header=Header(stamp=rospy.Time.now(), frame_id=mylimb+'_lower_shoulder'),
pose=userPose(user_pos, user_rot),
)
solve_move_trac(mylimb, leftArmPose)
#Refresh rate
rate.sleep()
Create goal Pose and call ik move
'''
pose_right = Pose(
position=Point(
x=pos.x(),
y=pos.y(),
z=pos.z(),
),
orientation=Quaternion(x=0, y=1, z=0, w=0),
)
return pose_right
#time.sleep(2)
init_pos = Vectors.V4D(0.8, -0.47, 0.2, 0)
bound = Vectors.V4D(0.656982770038, -0.252598021641, 0.5388609422173, 0)
init_pos2 = Vectors.V4D(0.656982770038, -0.35, 0.1, 0)
init_pos3 = Vectors.V4D(0.656982770038, -0.35, 0.4, 0)
myps = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=pose2(init_pos),
)
print 'initialisation move...'
resp = trac_ik_solve(mylimb, myps)
if resp is not None:
