def main():
print("init node")
rospy.init_node("baxter_joint_pos_set")
left = bi.Limb('left')
right = bi.Limb('right')
rate = rospy.Rate(1000)
state = bs.STATE()
ref = bs.STATE()
# for arm in range(0,bs.NUM_ARMS):
# for joint in range(0,bs.BAXTER_ARM_JOINTS_NUM):
# ref.arm[arm].joint[joint].ref = 0.0
# ref.arm[arm].joint[joint].pos = 0.0
# ref.arm[arm].joint[joint].tor = 0.0
ref.arm[bs.RIGHT].joint[bs.WY2].ref = 2.0
moveArm(ref, bs.RIGHT, right)
state = getState(state,ref,left,right)
ref.arm[bs.LEFT].joint[bs.WY2].ref = 1.0
moveArm(ref, bs.LEFT, left)
state = getState(state,ref,left,right)
print left.joint_angle('left_w2')
print state.arm[bs.LEFT].joint[bs.WY2].pos
print state.arm[bs.LEFT].joint[bs.WY2].ref
print right.joint_angle('right_e1')
import ach
import time
import argparse
import sys
import rospy
import baxter_interface as bi
import baxterStructure as bs
from ctypes import *
data_out = ach.Channel('state_channel')
data_in = ach.Channel('ref_channel')
state = bs.STATE()
ref = bs.STATE()
data_in.flush()
print("init node")
rospy.init_node("baxter_joint_pos_set")
right = bi.Limb('right')
left = bi.Limb('left')
rate = rospy.Rate(1000)
def moveArm(ref2, arm, limb):
ref = bs.A2B(ref2)
if arm == bs.RIGHT:
angles = {
'right_s0': ref.arm[bs.RIGHT].joint[bs.SY].ref,
'right_s1': ref.arm[bs.RIGHT].joint[bs.SP].ref,
'right_w0': ref.arm[bs.RIGHT].joint[bs.WY].ref,
'right_w1': ref.arm[bs.RIGHT].joint[bs.WP].ref,
def main():
s = ach.Channel(bs.STATE_CHANNEL)
r = ach.Channel(bs.REF_CHANNEL)
print("init node")
rospy.init_node("baxter_joint_pos_set")
left = bi.Limb('left')
right = bi.Limb('right')
rate = rospy.Rate(1000)
state = bs.STATE()
ref = bs.STATE()
[statuss, framesize] = s.get(state, wait=False, last=False)
lTheta = np.zeros((7, 1))
rTheta = np.zeros((7, 1))
rTheta[0, 0] = 0.0
rTheta[1, 0] = 0.0
rTheta[2, 0] = 0.0
rTheta[3, 0] = 0.0
rTheta[4, 0] = 0.0
rTheta[5, 0] = 0.0
rTheta[6, 0] = 0.0
print 'RIGHT FK', getFK(bs.RIGHT, rTheta)
lTheta[0, 0] = 0.0
lTheta[1, 0] = 0.0
lTheta[2, 0] = 0.0
lTheta[3, 0] = 0.0
lTheta[4, 0] = 0.0
lTheta[5, 0] = 0.0
lTheta[6, 0] = 0.0
print 'LEFT FK', getFK(bs.LEFT, lTheta)
rx = float(raw_input("rX: "))
ry = float(raw_input("rY: "))
rz = float(raw_input("rZ: "))
rGoal = np.array([[rx], [ry], [rz]])
getIK(bs.RIGHT, rTheta, rGoal, ref, r, right)
lx = float(raw_input("lX: "))
ly = float(raw_input("lY: "))
lz = float(raw_input("lZ: "))
lGoal = np.array([[lx], [ly], [lz]])
getIK(bs.LEFT, lTheta, lGoal, ref, r, left)
# ref.arm[bs.RIGHT].joint[bs.WY12].ref = 3.0
# moveArm(ref, bs.RIGHT, right)
# state = getState(state,ref,left,right)
# r.put(ref)
# ref.arm[bs.LEFT].joint[bs.WY2].ref = 3.0
# moveArm(ref, bs.LEFT, left)
# state = getState(state,ref,left,right)
# r.put(ref)
#
# print left.joint_angle('left_w2')
# print state.arm[bs.LEFT].joint[bs.WY2].pos
# print state.arm[bs.LEFT].joint[bs.WY2].ref
# print right.joint_angle('right_e1')
s.close()
r.close()