def getpose():
pub_joint_cmd=rospy.Publisher('/robot/limb/right/joint_command',JointCommand)
command_msg=JointCommand()
command_msg.names=['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
command_msg.mode=JointCommand.POSITION_MODE
control_rate = rospy.Rate(100)
start = rospy.get_time()
joint_positions=rospy.wait_for_message("/robot/joint_states",JointState)
qc = (joint_positions.position[9:16])
angles = [qc[2],qc[3],qc[0],qc[1],qc[4],qc[5],qc[6]]
pub_joint_cmd=rospy.Publisher('/robot/limb/right/joint_command',JointCommand)
command_msg=JointCommand()
command_msg.names=['right_e1']
command_msg.command=[0]
command_msg.mode=JointCommand.POSITION_MODE
control_rate = rospy.Rate(100)
start = rospy.get_time()
exit = 0
listener2=tf.TransformListener()
#the transformations
now=rospy.Time()
listener2.waitForTransform("/torso","/right_hand",now,rospy.Duration(1.0))
(trans08,rot08)=listener2.lookupTransform("/torso","/right_hand",now)
# Get 4*4 rotational matrix from quaternion ( 4th colume is [0][0][0][1])
R08 = transformations.quaternion_matrix(rot08)
T08 = numpy.vstack((numpy.column_stack((R08[0:3,0:3], numpy.transpose(numpy.array(trans08)))),[0,0,0,1]))
return (angles, T08)
def send_to_joint_vals(q, arm='right'):
pub_joint_cmd = rospy.Publisher('/robot/limb/' + arm + '/joint_command',
JointCommand)
command_msg = JointCommand()
command_msg.names = generate_joint_names_for_arm(arm)
command_msg.command = q
command_msg.mode = JointCommand.POSITION_MODE
control_rate = rospy.Rate(100)
start = rospy.get_time()
joint_positions = rospy.wait_for_message("/robot/joint_states", JointState)
qc = joint_positions.position[9:16]
qc = ([qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6]])
print('q,qc')
print(q)
print(qc)
while not rospy.is_shutdown() and numpy.linalg.norm(numpy.subtract(
q, qc)) > 0.07:
pub_joint_cmd.publish(
command_msg) # sends the commanded joint values to Baxter
control_rate.sleep()
joint_positions = rospy.wait_for_message("/robot/joint_states",
JointState)
qc = (joint_positions.position[9:16])
qc = (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6])
print "joint error = ", numpy.linalg.norm(numpy.subtract(q, qc))
print((q, qc))
print("In home pose")
return (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6])
def send_to_joint_vals(q, arm='right'):
pub_joint_cmd=rospy.Publisher('/robot/limb/' + arm + '/joint_command',JointCommand)
command_msg=JointCommand()
command_msg.names = generate_joint_names_for_arm(arm)
command_msg.command=q
command_msg.mode=JointCommand.POSITION_MODE
control_rate = rospy.Rate(100)
start = rospy.get_time()
joint_positions=rospy.wait_for_message("/robot/joint_states",JointState)
qc = joint_positions.position[9:16]
qc = ([qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6]])
print('q,qc')
print(q)
print(qc)
while not rospy.is_shutdown() and numpy.linalg.norm(numpy.subtract(q,qc))> 0.07:
pub_joint_cmd.publish(command_msg) # sends the commanded joint values to Baxter
control_rate.sleep()
joint_positions=rospy.wait_for_message("/robot/joint_states",JointState)
qc = (joint_positions.position[9:16])
qc = (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6])
print "joint error = ", numpy.linalg.norm(numpy.subtract(q,qc))
print((q,qc))
print("In home pose")
return (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6])
def main():
rospy.init_node('jointPositionPublisherExample', anonymous=True)
pub_joint_cmd = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand,
queue_size=10)
# rospy.Subscriber('/robot/limb/' + 'left' +'/gravity_compensation_torques', SEAJointState, force_sensor_callback)
limb = baxter_interface.Limb("left")
limb.set_joint_position_speed(0.5)
limb.set_command_timeout(120.0)
cmdMsg = JointCommand()
cmdMsg.mode = JointCommand.POSITION_MODE
# q untucked, considered to be the initial q: 'left_w0': 0.669996718047412, 'left_w1': 1.030008750129423, 'left_w2': -0.4999996908801263, 'left_e0': -1.1899720096239292, 'left_e1': 1.940029476507764, 'left_s0': -0.08000000085054282, 'left_s1': -0.9999845808672081
q = limb.joint_angles() #joint configuration
q['left_w2'] = 3
cmdMsg.command = q.values()
cmdMsg.names = q.keys()
print(cmdMsg)
pub_joint_cmd.publish(cmdMsg)
#positions = dict(zip(self.limb.joint_names(), theta))
rate = rospy.Rate(2)
while not rospy.is_shutdown():
printCurJointPos(limb)
pub_joint_cmd.publish(cmdMsg)
rate.sleep()
def move_list_smooth_wobble(neutral=False,
arm=None,
p_list=0,
timeout=default_timeout,
threshold=default_threshold,
speed=default_speed):
arm = arm.lower()
if speed > 1.0:
speed = 1.0
elif speed <= 0.0:
speed = 0.1
if arm == 'l':
arm = 'left'
elif arm == 'r':
arm = 'right'
if arm == 'left' or arm == 'right':
limb = baxter_interface.Limb(arm)
limb.set_joint_position_speed(speed)
if neutral:
limb.move_to_neutral()
for positions in p_list:
position = dictToList(limb, positions)
pub_joints = rospy.Publisher('/robot/limb/' + arm +
'/joint_command',
JointCommand,
queue_size=5)
cmd_msg = JointCommand()
cmd_msg.mode = JointCommand.POSITION_MODE
cmd_msg.names = [
arm + '_w0', arm + '_w1', arm + '_w2', arm + '_e0',
arm + '_e1', arm + '_s0', arm + '_s1'
]
cmd_msg.names = [
arm + '_w0', arm + '_w1', arm + '_w2', arm + '_e0',
arm + '_e1', arm + '_s0', arm + '_s1'
]
bo_finish = False
while not bo_finish and not rospy.is_shutdown():
cmd_msg.command = position
pub_joints.publish(cmd_msg)
time.sleep(0.19)
bo_finish = not checkThreshold(limb, position, threshold)
if neutral:
limb.move_to_neutral()
limb.set_joint_position_speed(default_speed)
else:
print("Error: {} is not a valid limb".format(arm))
def stopMoving(self):
lcmd = JointCommand()
rcmd = JointCommand()
lcmd.names = self.baxter_larm_names
rcmd.names = self.baxter_rarm_names
lcmd.mode = rcmd.mode = POSITION_MODE
lcmd.command = []
for n in lcmd.names:
index = self.currentJointStates.name.index(n)
lcmd.command.append(self.currentJointStates.position[index])
rcmd.command = []
for n in rcmd.names:
index = self.currentJointStates.name.index(n)
rcmd.command.append(self.currentJointStates.position[index])
self.pub_larm.publish(lcmd)
self.pub_rarm.publish(rcmd)
self.pub_hnod.publish(False)
def stopMoving(self):
lcmd = JointCommand()
rcmd = JointCommand()
lcmd.names = self.baxter_larm_names
rcmd.names = self.baxter_rarm_names
lcmd.mode = rcmd.mode = POSITION_MODE
lcmd.command = []
for n in lcmd.names:
index = self.currentJointStates.name.index(n)
lcmd.command.append(self.currentJointStates.position[index])
rcmd.command = []
for n in rcmd.names:
index = self.currentJointStates.name.index(n)
rcmd.command.append(self.currentJointStates.position[index])
self.pub_larm.publish(lcmd)
self.pub_rarm.publish(rcmd)
self.pub_hnod.publish(False)
def runExperiment(Kp, Kd, Ki, commandHz):
bc = BaxterCache()
print("Resetting robot")
tucker = baxter_tools.Tuck(False)
rospy.on_shutdown(tucker.clean_shutdown)
tucker.supervised_tuck()
rospy.sleep(1)
controlTopic = "/robot/limb/right/joint_command"
pub = rospy.Publisher(controlTopic, JointCommand, queue_size=20)
messageObj = JointCommand()
messageObj.mode = JointCommand.TORQUE_MODE
messageObj.names = joint_names
rate = rospy.Rate(commandHz)
print("Waiting for first joint_state message")
while bc.joints is None:
rate.sleep()
print("First joint_state message acquired")
print("Waiting for first target_loc message")
while bc.get_desired_joints_and_vel() is None:
rate.sleep()
print("First target_loc message acquired")
time.sleep(1)
cumulativeJointError = np.zeros(bc.joints.shape)
fileName = "results/Results%s.csv" % ("Interp"
if bc.trackFuture else "Naive")
print("logging to %s" % fileName)
with StateLogger(fileName) as logger:
it = 0
maxIt = 2000 * commandHz
while not rospy.is_shutdown(): # and it < maxIt:
des = bc.get_desired_joints_and_vel()
if des is None or des[0] is None:
continue
desired_joints = des[0]
desired_dot = des[1]
# compute the current joint angles
err = bc.joints - desired_joints
# note this needs to be scaled by freq of messages
cumulativeJointError += err / 1000 * 2
err_dot = bc.joints_dot - desired_dot
# log at 40 hz
if it % int(max(1, math.floor(commandHz / 30))) == 0:
if it % 100 == 0:
print(it)
logger.log([it, rospy.get_rostime().to_sec()] +
bc.end_eff_state + bc.truth)
it += 1
torques = compute_torque(err, err_dot, cumulativeJointError, Kp,
Kd, Ki)
messageObj.command = [float(torque) for torque in torques.tolist()]
pub.publish(messageObj)
rate.sleep()
def _send_pos_command(self, pos):
self._try_enable()
command = JointCommand()
command.mode = JointCommand.POSITION_MODE
command.names = self._limb.joint_names()
#command.position = pos
command.command = pos
self._cmd_publisher.publish(command)
def move_to_ja(self, waypoints, duration=1.5):
"""
:param waypoints: List of joint angle arrays. If len(waypoints) == 1: then go directly to point.
Otherwise: take trajectory that ends at waypoints[-1] and passes through each intermediate waypoint
:param duration: Total time trajectory will take before ending
"""
self._try_enable()
jointnames = self._limb.joint_names()
prev_joint = np.array([self._limb.joint_angle(j) for j in jointnames])
waypoints = np.array([prev_joint] + waypoints)
spline = CSpline(waypoints, duration)
start_time = rospy.get_time() # in seconds
finish_time = start_time + duration # in seconds
time = rospy.get_time()
while time < finish_time:
pos, velocity, acceleration = spline.get(time - start_time)
command = JointCommand()
command.mode = JointCommand.POSITION_MODE
command.names = jointnames
command.command = pos
#command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag)
#command.acceleration = np.clip(acceleration, -max_accel_mag, max_accel_mag)
self._cmd_publisher.publish(command)
self._control_rate.sleep()
time = rospy.get_time()
for i in xrange(10):
command = JointCommand()
command.mode = JointCommand.POSITION_MODE
command.names = jointnames
#command.position = waypoints[-1]
command.command = waypoints[-1]
self._cmd_publisher.publish(command)
self._control_rate.sleep()
def read_file(file):
global joint_command_publisher
global g_limb
global g_left_gripper
global g_endpoint_state
f=open(file,"r")
lines=f.readlines()
RATE=0.1
cmd=JointCommand()
cmd.mode=1
g_left_gripper.open()
names=["left_s0","left_s1","left_e0","left_e1","left_w0","left_w1","left_w2"]
cmd.names=names
print cmd
joint_str=lines[0].rstrip().split(',')
joints=[float(joint) for joint in joint_str]
limb_pos=dict(zip(names,joints[0:7]))
is_grasping=is_grasp(joints[7])
if(is_grasping):
g_left_gripper.close()
else:
g_left_gripper.open()
g_limb.move_to_joint_positions(limb_pos)
i=1
while i<len(lines) and not rospy.is_shutdown():
line=lines[i]
joint_str=line.rstrip().split(',')
joints=[float(joint) for joint in joint_str]
if is_grasping != is_grasp(joints[7]):
if is_grasp(joints[7]):
g_left_gripper.close()
is_grasping=True
else:
g_left_gripper.open()
cmd.command=joints[0:7]
# print joints[0:7]
print i
joint_command_publisher.publish(cmd)
time.sleep(RATE)
i=i+1
def publishIKdata(joint_vals, limb):
# available control modes:
# POSITION_MODE=1
# VELOCITY_MODE=2
# TORQUE_MODE=3
# RAW_POSITION_MODE=4
msg = JointCommand()
msg.names = joint_vals.keys()
msg.command = joint_vals.values()
msg.mode = JointCommand.POSITION_MODE
if limb == 'left':
pub_left.publish(msg)
if limb == 'right':
pub_right.publish(msg)
def left_pub(commands, names):
global left_command_pub
"""
This function sets your speed for the publisher. Use this function to implement your ideas.
:params:
x:This is the velocity along x-axis in m/sec. Limits: [-0.3,0.3]
y:This is the velocity along y-axis in m/sec. Limits: [-0.3,0.3]
wz: This is the angular velocity for turning (yaw) in rad/sec. Limits: [-1,1]
:return: Returns a speed message
"""
joint_cmd = JointCommand()
joint_cmd.mode = 1
joint_cmd.command = commands
joint_cmd.names = names
left_command_pub.publish(joint_cmd)
def callback(comm):
msg = PoseStamped()
msg.header.seq = 0
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = 'base'
msg.pose = comm.pose
arr = SolvePositionIKRequest()
arr.pose_stamp.append(msg)
arr.seed_mode = 0
print arr
left_ik_addr = '/ExternalTools/left/PositionKinematicsNode/IKService'
right_ik_addr = '/ExternalTools/right/PositionKinematicsNode/IKService'
if rospy.get_param('/robot/unityros/movelock') == True:
return
try:
left_ik = rospy.ServiceProxy(left_ik_addr, SolvePositionIK)
right_ik = rospy.ServiceProxy(right_ik_addr, SolvePositionIK)
if comm.limb == 'left':
ik = left_ik(arr)
else:
ik = right_ik(arr)
right_limb = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=1)
left_limb = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand,
queue_size=1)
message = JointCommand()
message.mode = 1
message.command = ik.joints[0].position
message.names = ik.joints[0].name
rospy.loginfo(message)
right_limb.publish(message)
left_limb.publish(message)
except rospy.ServiceException, e:
print 'Service call failed: %s' % e
def main():
rospy.init_node('baxter_arm')
rospy.loginfo('main method called')
rate = rospy.Rate(10)
pub = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand,
queue_size=1)
while not rospy.is_shutdown():
msg = JointCommand()
msg.mode = msg.POSITION_MODE
msg.names = {
'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2',
'right_e0', 'right_e1'
}
msg.command = {0.0, 0.0, 0.1, 0.1, 0.0, 0.0, 0.0}
pub.publish(msg)
rate.sleep()
def callback(data):
time_now = rospy.Time().now()
if (time_now.secs - data.header.stamp.secs >
1 | time_now.nsecs - data.header.stamp.nsecs > 500000000):
return
print "accept command"
commandPose = data.pose
limb_name = data.header.frame_id
limb = baxter_interface.Limb(limb_name)
currentPose = limb.endpoint_pose()
while not ("position" in currentPose):
currentPose = limb.endpoint_pose()
newPose = dict()
newPose = {
'position':
Point(
commandPose.position.x,
commandPose.position.y,
commandPose.position.z,
),
'orientation':
Quaternion(
commandPose.orientation.x,
commandPose.orientation.y,
commandPose.orientation.z,
commandPose.orientation.w,
),
}
kinematics = baxter_kinematics(limb_name)
inverseKinematics = kinematics.inverse_kinematics(newPose["position"],
newPose["orientation"])
if not (inverseKinematics == None):
inverseKinematicsSolution = list()
for num in inverseKinematics.tolist():
inverseKinematicsSolution.append(num)
inverseKinematicsSolutionJointCommand = JointCommand()
inverseKinematicsSolutionJointCommand.mode = 1
inverseKinematicsSolutionJointCommand.names = limb.joint_names()
inverseKinematicsSolutionJointCommand.command = inverseKinematicsSolution
pub.publish(inverseKinematicsSolutionJointCommand)
def callback(data):
#time_now = rospy.Time().now()
#if (time_now.secs - data.header.stamp.secs > 1):
# rospy.loginfo("time stamp too old, ignore...")
# return
commandPose = data.pose
limb_name = data.header.frame_id;
limb = baxter_interface.Limb(limb_name)
newPose = dict()
newPose = {
'position': Point(
commandPose.position.x,
commandPose.position.y,
commandPose.position.z,
),
'orientation': Quaternion(
commandPose.orientation.x,
commandPose.orientation.y,
commandPose.orientation.z,
commandPose.orientation.w,
),
}
kinematics = baxter_kinematics(limb_name)
inverseKinematics = kinematics.inverse_kinematics(newPose["position"], newPose["orientation"])
if not (inverseKinematics == None):
inverseKinematicsSolution = list()
for num in inverseKinematics.tolist():
inverseKinematicsSolution.append(num)
inverseKinematicsSolutionJointCommand = JointCommand()
inverseKinematicsSolutionJointCommand.mode = 1
inverseKinematicsSolutionJointCommand.names = limb.joint_names()
inverseKinematicsSolutionJointCommand.command = inverseKinematicsSolution
commandSolutionPublisher.publish(inverseKinematicsSolutionJointCommand)
# publish to command subscriber to check compute
commandCheckPublisher.publish(String("done"))
def callback(data):
time_now = rospy.Time().now()
if (time_now.secs - data.header.stamp.secs > 1 | time_now.nsecs - data.header.stamp.nsecs > 500000000):
return
print "accept command"
commandPose = data.pose
limb_name = data.header.frame_id;
limb = baxter_interface.Limb(limb_name)
currentPose = limb.endpoint_pose()
while not ("position" in currentPose):
currentPose = limb.endpoint_pose()
newPose = dict()
newPose = {
'position': Point(
commandPose.position.x,
commandPose.position.y,
commandPose.position.z,
),
'orientation': Quaternion(
commandPose.orientation.x,
commandPose.orientation.y,
commandPose.orientation.z,
commandPose.orientation.w,
),
}
kinematics = baxter_kinematics(limb_name)
inverseKinematics = kinematics.inverse_kinematics(newPose["position"], newPose["orientation"])
if not (inverseKinematics == None):
inverseKinematicsSolution = list()
for num in inverseKinematics.tolist():
inverseKinematicsSolution.append(num)
inverseKinematicsSolutionJointCommand = JointCommand()
inverseKinematicsSolutionJointCommand.mode = 1
inverseKinematicsSolutionJointCommand.names = limb.joint_names()
inverseKinematicsSolutionJointCommand.command = inverseKinematicsSolution
pub.publish(inverseKinematicsSolutionJointCommand)
def getpose():
pub_joint_cmd = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand)
command_msg = JointCommand()
command_msg.names = [
'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1',
'right_w2'
]
command_msg.mode = JointCommand.POSITION_MODE
control_rate = rospy.Rate(100)
start = rospy.get_time()
joint_positions = rospy.wait_for_message("/robot/joint_states", JointState)
qc = (joint_positions.position[9:16])
angles = [qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6]]
pub_joint_cmd = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand)
command_msg = JointCommand()
command_msg.names = ['right_e1']
command_msg.command = [0]
command_msg.mode = JointCommand.POSITION_MODE
control_rate = rospy.Rate(100)
start = rospy.get_time()
exit = 0
listener2 = tf.TransformListener()
#the transformations
now = rospy.Time()
listener2.waitForTransform("/torso", "/right_hand", now,
rospy.Duration(1.0))
(trans08, rot08) = listener2.lookupTransform("/torso", "/right_hand", now)
# Get 4*4 rotational matrix from quaternion ( 4th colume is [0][0][0][1])
R08 = transformations.quaternion_matrix(rot08)
T08 = numpy.vstack((numpy.column_stack(
(R08[0:3, 0:3], numpy.transpose(numpy.array(trans08)))), [0, 0, 0, 1]))
return (angles, T08)
def process(self):
if self.currentAssemblyState==None:
#not enabled, return
print "Waiting for /robot/state..."
time.sleep(1.0)
return
if self.currentAssemblyState.stopped:
print "Robot is stopped"
time.sleep(1.0)
return
if self.currentJointStates==None:
print "Waiting for joint states to be published..."
time.sleep(1.0)
return
if self.lastUpdateTime == None:
self.lastUpdateTime = self.currentJointStates.header.stamp
return
self.currentTime = self.currentJointStates.header.stamp
#convert to Klamp't message
klamptInput = dict()
klamptInput['t'] = self.currentTime.to_sec()
klamptInput['dt'] = self.currentTime.to_sec() - self.lastUpdateTime.to_sec()
if klamptInput['dt'] == 0.0:
#no new clock messages read
return
klamptInput['q'] = [0.0]*self.robot_model.numLinks()
klamptInput['dq'] = [0.0]*self.robot_model.numLinks()
klamptInput['torque'] = [0.0]*self.robot_model.numDrivers()
for i,n in enumerate(self.currentJointStates.name):
if n not in self.nameToLinkIndex:
if n != 'torso_t0':
print "Ignoring state of link",n
continue
klamptIndex = self.nameToLinkIndex[n]
klamptInput['q'][klamptIndex] = self.currentJointStates.position[i]
if len(self.currentJointStates.velocity) > 0:
klamptInput['dq'][klamptIndex] = self.currentJointStates.velocity[i]
if len(self.currentJointStates.effort) > 0:
driverIndex = self.nameToDriverIndex[n]
klamptInput['torque'][driverIndex] = self.currentJointStates.effort[i]
#if self.currentHeadState != None:
# klamptInput['q'][self.head_pan_link_index] = self.currentHeadState.pan
#output is defined in SerialController
klamptReply = self.output(**klamptInput)
if klamptReply == None:
return False
#now conver the Klamp't reply to a Baxter command
lcmd = JointCommand()
rcmd = JointCommand()
lcmd.names = self.baxter_larm_names
rcmd.names = self.baxter_rarm_names
hpcmd = HeadPanCommand()
hncmd = Bool()
if 'qcmd' in klamptReply:
#use position mode
lcmd.mode = rcmd.mode = POSITION_MODE
q = klamptReply['qcmd']
lcmd.command = [q[self.nameToLinkIndex[n]] for n in lcmd.names]
rcmd.command = [q[self.nameToLinkIndex[n]] for n in rcmd.names]
hpcmd.target = q[self.head_pan_link_index]
hpcmd.speed = q[self.head_pan_link_index]
elif 'dqcmd' in klamptReply:
lcmd.mode = rcmd.mode = VELOCITY_MODE
dq = klamptReply['dqcmd']
lcmd.command = [dq[self.nameToLinkIndex[n]] for n in lcmd.names]
rcmd.command = [dq[self.nameToLinkIndex[n]] for n in rcmd.names]
hpcmd = None
elif 'torquecmd' in klamptReply:
lcmd.mode = rcmd.mode = TORQUE_MODE
torque = klamptReply['torquecmd']
lcmd.command = [torque[self.nameToDriverIndex[n]] for n in lcmd.names]
rcmd.command = [torque[self.nameToDriverIndex[n]] for n in rcmd.names]
hpcmd = None
else:
lcmd = rcmd = None
hpcmd = None
hncmd = None
if self.currentAssemblyState.estop_button != AssemblyState.ESTOP_BUTTON_UNPRESSED:
print "Estop is on..."
time.sleep(1.0)
elif not self.currentAssemblyState.enabled:
print "Waiting for robot to turn on..."
time.sleep(1.0)
else:
#publish the messages
if lcmd:
self.pub_larm.publish(lcmd)
if rcmd:
self.pub_rarm.publish(rcmd)
if hpcmd:
self.pub_hpan.publish(hpcmd)
if hncmd:
self.pub_hnod.publish(hncmd)
self.lastUpdateTime = self.currentTime
return True
def main():
rospy.init_node('trayectoria_circular')
#Ponemos RATE de mensaje
rate = rospy.Rate(20)
#Creamos objeto PUBLICADOR
pub = rospy.Publisher('/robot/limb/left/joint_command',JointCommand,queue_size=10)
arg_fmt = argparse.ArgumentDefaultsHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt)
parser.add_argument(
"-l", "--limb", dest="limb", default="left",
choices=['left', 'right'],
help="joint trajectory limb"
)
parser.add_argument(
"-R", "--radius", dest="radius", default=0.2,
type=float,
help="trajectory radius"
)
parser.add_argument(
"-v", "--velocity", dest="velocity", default=5.0,
type=float, help="trajectory velocity"
)
parser.add_argument(
"-r", "--revolution", dest="revolution", default=0.9,
type=float, help="Revolutions"
)
args = parser.parse_args(rospy.myargv()[1:])
#print(sys.argv[1]);
kin = baxter_kinematics(args.limb)
limb = baxter_interface.Limb(args.limb)
timeout = args.velocity/10.0
radio = args.radius
decremento = 0.001
posiciones = [];
centro_y = 0.35
centro_z = 0.1
if args.limb == 'right':
centro_y = centro_y * (-1)
# FK Position
limb.move_to_neutral()
kp = kin.forward_position_kinematics()
#Array donde vamos acumulando configuraciones q1,q2,...,qn para seguir la trayectoria
#Util para control por Torques con PID
posiciones.append(kp);
#Centro de la Circunferencia
pos = [kp[0],kp[1]-centro_y,kp[2]+centro_z]
rot = [kp[3],kp[4],kp[5],kp[6]]
#Calculamos configuracion Qcentro
l = kin.inverse_kinematics(pos, rot)
m = {args.limb +'_w0': l[4], args.limb + '_w1': l[5], args.limb +'_w2': l[6], args.limb +'_e0': l[2], args.limb +'_e1': l[3], args.limb +'_s0': l[0], args.limb +'_s1': l[1]}
#Movemos al centro de la circunferencia
limb.move_to_joint_positions(m)
posiciones.append(l);
puntos_circunferencia = []
puntos_circunferencia2 = []
#Calculamos los valores que tomara Z y que nos servira para calcular Y posteriormente
valoresTomar = []
i = 1
while(i >= -0.5):
valoresTomar.append(i)
i = i - decremento
#Calculamos las posiciones para describir circunferencia en plano ZY
for z in valoresTomar:
c = 2*pos[1]
b = (-1)*(-z*z + 2*z*pos[2] + radio*radio - pos[2]*pos[2] - pos[1]*pos[1])
if c*c - 4*b > 0:
y = (c + sqrt(c*c - 4*b)) / 2
y2 = (c - sqrt(c*c - 4*b)) / 2
puntos_circunferencia.append([y,z])
puntos_circunferencia2.append([y2,z])
Posiciones = []
Rotaciones = []
ErroresPos = []
ErroresRot = []
#Vamos PUBLICANDO las posiciones que deseamos alcanzar para seguir la trayectoria
#Creamos MENSAJE
msg = JointCommand()
#Modo - 1 (CONTROL POR POSICION)
msg.mode = 1
msg.names = [args.limb +'_w0',args.limb + '_w1',args.limb +'_w2',args.limb +'_e0',args.limb +'_e1',args.limb +'_s0',args.limb +'_s1']
for p in puntos_circunferencia2:
kp = kin.forward_position_kinematics()
#Calculamos (x,y,z)
pos = [kp[0],p[0],p[1]]
#Calculamos tupla Orientacion (i,j,w,k)
rot = [kp[3],kp[4],kp[5],kp[6]]
#Calculamos (theta1,...) para (x,y,z)
l = kin.inverse_kinematics(pos, rot)
#Posicion Valida
if l != None:
Posiciones.append(pos)
Rotaciones.append(rot)
msg.command = [l[4],l[5],l[6],l[2],l[3],l[0],l[1]]
pub.publish(msg)
rate.sleep()
ErroresRot.append(rot)
posiciones.append(l);
else:
print(p);
ErroresPos.append(pos)
puntos_circunferencia = puntos_circunferencia[::-1]
for p in puntos_circunferencia:
kp = kin.forward_position_kinematics()
pos = [kp[0],p[0],p[1]]
rot = [kp[3],kp[4],kp[5],kp[6]]
l = kin.inverse_kinematics(pos, rot)
if l != None:
Posiciones.append(pos)
Rotaciones.append(rot)
msg.command = [l[4],l[5],l[6],l[2],l[3],l[0],l[1]]
pub.publish(msg)
rate.sleep()
posiciones.append(l);
else:
ErroresPos.append(pos)
ErroresRot.append(rot)
#Caso de que queramos realizar la trayectoria en mas de una ocasion
n_vueltas = args.revolution
k = 1
while k < n_vueltas:
for i in range(len(Posiciones)):
pos = Posiciones[i]
rot = Rotaciones[i]
l = kin.inverse_kinematics(pos, rot)
msg.command = [l[4],l[5],l[6],l[2],l[3],l[0],l[1]]
pub.publish(msg)
rate.sleep()
k = k+1
np.save('trayectoriaC',np.array(posiciones));
def main():
rospy.init_node('trayectoria_sinusoidal')
#Ponemos RATE de mensaje
rate = rospy.Rate(1)
#Creamos objeto PUBLICADOR
pub = rospy.Publisher('/robot/limb/left/joint_command',JointCommand,queue_size=10)
arg_fmt = argparse.ArgumentDefaultsHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt)
parser.add_argument(
"-l", "--limb", dest="limb", default="left",
choices=['left', 'right'],
help="joint trajectory limb"
)
parser.add_argument(
"-v", "--velocity", dest="velocity", default=5.0,
type=float, help="trajectory velocity"
)
parser.add_argument(
"-r", "--revolution", dest="revolution", default=10.0,
type=float, help="Revolutions"
)
parser.add_argument(
"-t", "--dt", dest="dt", default=0.025,
type=float, help="Differential T"
)
parser.add_argument(
"-n", "--points", dest="n", default=100,
type=int, help="Number of movements"
)
args = parser.parse_args(rospy.myargv()[1:])
kin = baxter_kinematics(args.limb)
limb = baxter_interface.Limb(args.limb)
timeout = args.velocity/10.0
n_mov = args.n
dt = args.dt
t = 0
#Movemos a posicion neutral
limb.move_to_neutral();
#Creamos MENSAJE
msg = JointCommand()
#Modo - 1 (CONTROL POR POSICION)
msg.mode = 1
msg.names = [args.limb +'_w0',args.limb + '_w1',args.limb +'_w2',args.limb +'_e0',args.limb +'_e1',args.limb +'_s0',args.limb +'_s1']
#Realizamos trayectoria sinusoidal
contador = 0;
while(contador < n_mov):
kp = kin.forward_position_kinematics()
x = 0.5
y = t/100
z = 0.1*sin(3*t)
if(args.limb == 'right'): y = y*(-1);
pos = [kp[0],kp[1]-y,kp[2]-z]
rot = [kp[3],kp[4],kp[5],kp[6]]
theta_i = kin.inverse_kinematics(pos,rot);
if theta_i != None:
msg.command = [theta_i[4],theta_i[5],theta_i[6],theta_i[2],theta_i[3],theta_i[0],theta_i[1]]
pub.publish(msg) #Publicamos el mensaje
rate.sleep() #Esperamos para conseguir el rate deseado
t = t + dt
contador = contador + 1;
def calcularFitness(individual, qObjs):
fitness = 0
KP = np.array(individual[0:7])
KD = np.array(individual[7:14])
KI = np.array(individual[14:21])
m = {
'left_w0': 0.0,
'left_w1': 0.0,
'left_w2': 0.0,
'left_e0': 0.0,
'left_e1': 0.0,
'left_s0': -1.0,
'left_s1': 0.0
}
limb.move_to_joint_positions(m)
#Creamos MENSAJE
msg = JointCommand()
#Modo - 1 (CONTROL POR POSICION)
msg.mode = 3
msg.names = [
'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1',
'left_w2'
]
errores = []
errores2 = []
dt = []
t = 0
i = 0
q_sensores = []
kinematics = baxter_kinematics('left')
while i < len(qObjs):
dt.append(time.clock())
qDes = qObjs[i]
angles = limb.joint_angles()
q = np.array([
angles['left_s0'], angles['left_s1'], angles['left_e0'],
angles['left_e1'], angles['left_w0'], angles['left_w1'],
angles['left_w2']
])
q_sensores.append(q)
error = np.array(qDes - q)
errores.append(error)
if i != 0:
der = (errores[len(errores) - 1] - errores[len(errores) - 2]) / (
dt[len(dt) - 2] - dt[len(dt) - 1])
torque = KP * error + KD * np.array(der)
else:
der = errores[len(errores) - 1]
torque = KP * error + KD * np.array(der)
m = {
'left_w0': torque[4],
'left_w1': torque[5],
'left_w2': torque[6],
'left_e0': torque[2],
'left_e1': torque[3],
'left_s0': torque[0],
'left_s1': torque[1]
}
msg.command = [
torque[0], torque[1], torque[2], torque[3], torque[4], torque[5],
torque[6]
]
pub.publish(msg) #Publicamos el mensaje
rate.sleep() #Esperamos para conseguir el rate deseado
#limb.set_joint_torques(m)
for l in range(len(error)):
fitness = fitness + error[l] * error[l]
i = i + 10
print(fitness)
return fitness
def execute_path(data_path):
left_limb_commander = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand,
queue_size=10)
right_limb_commander = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
head_pan_commander = rospy.Publisher('/robot/head/command_head_pan',
HeadPanCommand,
queue_size=10)
head_nod_commander = rospy.Publisher('/robot/head/command_head_nod',
Bool,
queue_size=10)
rospy.init_node('joint_commander', anonymous=True)
traj = get_trajectory(data_path)
for joint_state in traj:
left_limb_joint_names = []
left_limb_joint_values = []
right_limb_joint_names = []
right_limb_joint_values = []
head_pan_target = None
head_nod = False
joint_state = literal_eval(joint_state)
for joint_name, state in joint_state.items():
if joint_name.startswith('left'):
left_limb_joint_names.append(joint_name)
left_limb_joint_values.append(state)
elif joint_name.startswith('right'):
right_limb_joint_names.append(joint_name)
right_limb_joint_values.append(state)
elif joint_name == "head_pan":
head_pan_target = state
elif joint_name == "head_nod":
if abs(state) > 0:
head_nod = True
else:
head_nod = False
left_command = JointCommand()
left_command.mode = 1
left_command.names = left_limb_joint_names
left_command.command = left_limb_joint_values
right_command = JointCommand()
right_command.mode = 1
right_command.names = right_limb_joint_names
right_command.command = right_limb_joint_values
head_pan_command = HeadPanCommand()
head_pan_command.target = head_pan_target
head_pan_command.speed = 0.1
head_nod_command = Bool()
head_nod_command.data = head_nod
left_limb_commander.publish(left_command)
right_limb_commander.publish(right_command)
head_pan_commander.publish(head_pan_command)
head_nod_commander.publish(head_nod_command)
rospy.loginfo("State reached...")
def main():
rospy.init_node('trayectoria_rombo')
#Definimos el RATE de publicacion de mensajes en ROS
rate = rospy.Rate(10)
#Creamos objeto PUBLICADOR
pub = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand,
queue_size=10)
arg_fmt = argparse.ArgumentDefaultsHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt)
parser.add_argument("-l",
"--limb",
dest="limb",
default="left",
choices=['left', 'right'],
help="joint trajectory limb")
parser.add_argument("-v",
"--velocity",
dest="velocity",
default=5.0,
type=float,
help="trajectory velocity")
#Leemos los argumentos dados como entrada
args = parser.parse_args(rospy.myargv()[1:])
kin = baxter_kinematics(args.limb)
limb = baxter_interface.Limb(args.limb)
timeout = args.velocity / 10.0
n_mov = 32
# Movemos a posicion neutral
limb.move_to_neutral()
# Consultamos la posicion actual en formato : (x,y,z) y (i,j,w,k)
kp = kin.forward_position_kinematics()
#Limites del rombo a describir
limite_inferior_y = -1.5
limite_inferior_z = -0.8
proporcionY = limite_inferior_y / n_mov
proporcionZ = limite_inferior_z / n_mov
if args.limb == 'right':
proporcionY = proporcionY * (-1)
#Creamos MENSAJE
msg = JointCommand()
#Modo - 1 (CONTROL POR POSICION)
msg.mode = 1
msg.names = [
args.limb + '_w0', args.limb + '_w1', args.limb + '_w2',
args.limb + '_e0', args.limb + '_e1', args.limb + '_s0',
args.limb + '_s1'
]
#Vamos describiendo la trayectoria de Rombo
for i in range(n_mov):
kp = kin.forward_position_kinematics()
if i < n_mov // 2:
pos = [kp[0], kp[1] + proporcionY, kp[2] - proporcionZ]
else:
pos = [kp[0], kp[1] + proporcionY, kp[2] + proporcionZ]
rot = [kp[3], kp[4], kp[5], kp[6]]
l = kin.inverse_kinematics(pos, rot)
if l != None:
msg.command = [l[4], l[5], l[6], l[2], l[3], l[0], l[1]]
pub.publish(msg)
rate.sleep()
for i in range(n_mov):
kp = kin.forward_position_kinematics()
if i < n_mov // 2:
pos = [kp[0], kp[1] - proporcionY, kp[2] + proporcionZ]
else:
pos = [kp[0], kp[1] - proporcionY, kp[2] - proporcionZ]
rot = [kp[3], kp[4], kp[5], kp[6]]
theta_i = kin.inverse_kinematics(pos, rot)
if theta_i != None:
m = {
args.limb + '_w0': theta_i[4],
args.limb + '_w1': theta_i[5],
args.limb + '_w2': theta_i[6],
args.limb + '_e0': theta_i[2],
args.limb + '_e1': theta_i[3],
args.limb + '_s0': theta_i[0],
args.limb + '_s1': theta_i[1]
}
limb.move_to_joint_positions(m, timeout)
#!/usr/bin/env python
from numpy import *
import rospy
from baxter_core_msgs.msg import JointCommand
pub_joints = rospy.Publisher('/robot/limb/right/joint_command', JointCommand)
rospy.init_node('write_to_baxr', anonymous=False)
rate = rospy.Rate(20)
cmd_msg = JointCommand()
cmd_msg.mode = JointCommand.POSITION_MODE
cmd_msg.names = ['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
qini= [0.14035924209151535, -0.9794467330648366,1.588437105855346,0.39154859610775183,-0.8233641878974959, 0.7113835903818606, 0.8348690438066364]
while not rospy.is_shutdown():
cmd_msg.command = qini
pub_joints.publish(cmd_msg)
rate.sleep()
def process(self):
if self.currentAssemblyState == None:
#not enabled, return
print "Waiting for /robot/state..."
time.sleep(1.0)
return
if self.currentAssemblyState.stopped:
print "Robot is stopped"
time.sleep(1.0)
return
if self.currentJointStates == None:
print "Waiting for joint states to be published..."
time.sleep(1.0)
return
if self.lastUpdateTime == None:
self.lastUpdateTime = self.currentJointStates.header.stamp
return
self.currentTime = self.currentJointStates.header.stamp
#convert to Klamp't message
klamptInput = dict()
klamptInput['t'] = self.currentTime.to_sec()
klamptInput['dt'] = self.currentTime.to_sec(
) - self.lastUpdateTime.to_sec()
if klamptInput['dt'] == 0.0:
#no new clock messages read
return
klamptInput['q'] = [0.0] * self.robot_model.numLinks()
klamptInput['dq'] = [0.0] * self.robot_model.numLinks()
klamptInput['torque'] = [0.0] * self.robot_model.numDrivers()
for i, n in enumerate(self.currentJointStates.name):
if n not in self.nameToLinkIndex:
if n != 'torso_t0':
print "Ignoring state of link", n
continue
klamptIndex = self.nameToLinkIndex[n]
klamptInput['q'][klamptIndex] = self.currentJointStates.position[i]
if len(self.currentJointStates.velocity) > 0:
klamptInput['dq'][
klamptIndex] = self.currentJointStates.velocity[i]
if len(self.currentJointStates.effort) > 0:
driverIndex = self.nameToDriverIndex[n]
klamptInput['torque'][
driverIndex] = self.currentJointStates.effort[i]
#if self.currentHeadState != None:
# klamptInput['q'][self.head_pan_link_index] = self.currentHeadState.pan
#output is defined in SerialController
klamptReply = self.output(**klamptInput)
if klamptReply == None:
return False
#now conver the Klamp't reply to a Baxter command
lcmd = JointCommand()
rcmd = JointCommand()
lcmd.names = self.baxter_larm_names
rcmd.names = self.baxter_rarm_names
hpcmd = HeadPanCommand()
hncmd = Bool()
if 'qcmd' in klamptReply:
#use position mode
lcmd.mode = rcmd.mode = POSITION_MODE
q = klamptReply['qcmd']
lcmd.command = [q[self.nameToLinkIndex[n]] for n in lcmd.names]
rcmd.command = [q[self.nameToLinkIndex[n]] for n in rcmd.names]
hpcmd.target = q[self.head_pan_link_index]
hpcmd.speed = q[self.head_pan_link_index]
elif 'dqcmd' in klamptReply:
lcmd.mode = rcmd.mode = VELOCITY_MODE
dq = klamptReply['dqcmd']
lcmd.command = [dq[self.nameToLinkIndex[n]] for n in lcmd.names]
rcmd.command = [dq[self.nameToLinkIndex[n]] for n in rcmd.names]
hpcmd = None
elif 'torquecmd' in klamptReply:
lcmd.mode = rcmd.mode = TORQUE_MODE
torque = klamptReply['torquecmd']
lcmd.command = [
torque[self.nameToDriverIndex[n]] for n in lcmd.names
]
rcmd.command = [
torque[self.nameToDriverIndex[n]] for n in rcmd.names
]
hpcmd = None
else:
lcmd = rcmd = None
hpcmd = None
hncmd = None
if self.currentAssemblyState.estop_button != AssemblyState.ESTOP_BUTTON_UNPRESSED:
print "Estop is on..."
time.sleep(1.0)
elif not self.currentAssemblyState.enabled:
print "Waiting for robot to turn on..."
time.sleep(1.0)
else:
#publish the messages
if lcmd:
self.pub_larm.publish(lcmd)
if rcmd:
self.pub_rarm.publish(rcmd)
if hpcmd:
self.pub_hpan.publish(hpcmd)
if hncmd:
self.pub_hnod.publish(hncmd)
self.lastUpdateTime = self.currentTime
return True
def main():
rospy.init_node('kinect_test')
vive_data = []
rate = rospy.Rate(30)
rospy.Subscriber("unity_locations", String, vive_callback)
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
left_kin = baxter_kinematics('left')
right_kin = baxter_kinematics('right')
print('Program Starting! 10 Seconds to move to initial pose')
rospy.sleep(6.0)
print('Pose Acquired, Demonstrator must hold this position for now!!!!')
for i in vive:
vive_data.append(float(i))
human_data = np.array(vive_data)
print "human_data"
print human_data
c1 = human_data[0:7]
c2 = human_data[7:14]
head = human_data[14:21]
c1[0] = c1[0] - head[0]
c1[1] = c1[1] - head[1]
c1[2] = c1[2] - head[2]
c2[0] = c2[0] - head[0]
c2[1] = c2[1] - head[1]
c2[2] = c2[2] - head[2]
human_data = np.concatenate((c1, c2))
human_data = np.reshape(human_data, (1,14))
print human_data
right_json_file = open('right_nn_without_head.json', 'r')
right_loaded_model_json = right_json_file.read()
right_json_file.close()
right_joint_nn_model = model_from_json(right_loaded_model_json)
# load weights into new model
right_joint_nn_model.load_weights("right_nn_without_head_wt.h5")
right_joint_nn_model.compile(loss = 'mean_squared_error', optimizer='sgd')
right_predicted_joints = right_joint_nn_model.predict(human_data)[0]
print right_predicted_joints
left_neutral_position = {'left_s0':0.948767, 'left_s1':0.901981, 'left_e0':-1.456131, 'left_e1':0.541495, 'left_w0':0.887408, 'left_w1':0.488189, 'left_w2':-2.937190}
right_neutral_position = {'right_s0':-1.039272, 'right_s1':0.868233, 'right_e0':1.078005, 'right_e1':0.537660, 'right_w0':-3.045335, 'right_w1':-0.272282, 'right_w2':-1.091811}
left.move_to_joint_positions(left_neutral_position)
print "left completed"
right.move_to_joint_positions(right_neutral_position)
right.move_to_joint_positions({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]})
#print 'Want Robot Ready to Move?[y/n]',
#userInput = sys.stdin.readline().strip()
#if(userInput == 'y'):
#left.move_to_joint_positions(left_neutral_position)
#print "left completed"
#right.move_to_joint_positions(right_neutral_position)
#right.move_to_joint_positions({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]})
#else:
#print("Program exiting...")
#return
#print('Attempt real time tracking?[y/n]')
#if(userInput != 'y'):
# print("Program exiting...")
# return
pub_right = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10)
pub_left = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=10)
print('Realtime Tracking Starting! Demonstrator can move very slowly now')
while not rospy.is_shutdown():
vive_data = []
for i in vive:
vive_data.append(float(i))
human_data = np.array(vive_data)
c1 = human_data[0:7]
c2 = human_data[7:14]
head = human_data[14:21]
c1[0] = c1[0] - head[0]
c1[1] = c1[1] - head[1]
c1[2] = c1[2] - head[2]
c2[0] = c2[0] - head[0]
c2[1] = c2[1] - head[1]
c2[2] = c2[2] - head[2]
human_data = np.concatenate((c1, c2))
human_data = np.reshape(human_data, (1,14))
print human_data
right_predicted_joints = right_joint_nn_model.predict(human_data)[0]
#right_end_effector = right_kin.forward_position_kinematics({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]})
#print "end effector:"
#print right_end_effector
#with open("test_end_effector_vive.csv", 'a') as myfile:
# wr = csv.writer(myfile, quoting=csv.QUOTE_ALL)
# wr.writerow(np.concatenate((np.array(right_end_effector), human_data[0])))
print "predicted joints:"
print right_predicted_joints
#right.move_to_joint_positions({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]})
command_msg = JointCommand()
command_msg.mode = JointCommand.POSITION_MODE
command_msg.command = [right_predicted_joints[i] for i in xrange(0, 6)]
command_msg.names = ['right_s0', 'right_s1','right_e0','right_e1','right_w0','right_w1', 'right_w2']
pub_right.publish(command_msg)
rate.sleep()
while not rospy.is_shutdown():
tr.header.stamp = rospy.Time.now()
br.sendTransform(tr)
# update pose
Md[0, 3] = 0.05 * np.cos(t / 2.)
Md[1, 3] = 0.1 * np.sin(t / 2)
Md[2, 3] = 0.2 * np.cos(t / 2)
Md[:3, :3] = transformations.euler_matrix(.1 * np.cos(t),
.1 * np.sin(t / 2),
.05 * np.cos(t + 4))[:3, :3]
M = np.dot(M0, Md)
req.pose_stamp = [matrix_msg(M)]
res = ikr(req)
cmd.names = res.joints[0].name
cmd.command = res.joints[0].position
pubr.publish(cmd)
req.pose_stamp = [matrix_msg(np.dot(M, Mrl))]
res = ikl(req)
cmd.names = res.joints[0].name
cmd.command = res.joints[0].position
publ.publish(cmd)
t += dt
rospy.sleep(dt)
pos_00 = {'left_w0': -0.17602429520874024, 'left_w1': 0.8264321485290528, 'left_w2': 0.611674838470459, 'left_e0': 0.2320145939025879, 'left_e1': 0.9008302166564942, 'left_s0': -0.09357282795410157, 'left_s1': -0.22511168036499024}
pos_10 = {'left_w0': -0.5921165834472657, 'left_w1': 0.8302671004943848, 'left_w2': 0.6864564017944337, 'left_e0': 0.5372767703430176, 'left_e1': 1.03083508828125, 'left_s0': -0.36201946552734376, 'left_s1': -0.20133497817993165}
pos_20 = {'left_w0': -1.298898230657959, 'left_w1': 1.1255584018249511, 'left_w2': 0.8356360332458497, 'left_e0': 1.0526943144836427, 'left_e1': 1.0404224681945802, 'left_s0': -0.655009795678711, 'left_s1': 0.03911651004638672}
pos_1 = {'left_w0': 1.1662088926574707, 'left_w1': -0.2308641083129883, 'left_w2': -0.09625729432983399, 'left_e0': -1.2026409363281252, 'left_e1': -0.050237870745849615, 'left_s0': 0.3900146148742676, 'left_s1': 0.45559229348144537}
pos_2 = {'left_w0': 1.0956457764953613, 'left_w1': -1.4016749433288576, 'left_w2': -0.15569904979248048, 'left_e0': -1.2390729799987794, 'left_e1': -0.04947088035278321, 'left_s0': 0.390781605267334, 'left_s1': 0.12348545328369141}
pub_joints = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=1)
rospy.init_node("hand_wave")
rate = rospy.Rate(100)
cmd_msg = JointCommand()
cmd_msg.mode = JointCommand.POSITION_MODE
cmd_msg.names = ['left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0', 'left_s1']
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
left.set_joint_position_speed(0.5)
print(left.joint_velocities())
print(right.joint_velocities())
#left.set_joint_velocities({'left_w1' : 50})
while not rospy.is_shutdown():
if which_pos:
position = dictToList(pos_1)
else:
position = dictToList(pos_2)
cmd_msg.command = position
pub_joints.publish(cmd_msg)
print(cmd_msg)
rospy.sleep(2)
which_pos = not which_pos
