def vel_test():
jointCmdPub = rospy.Publisher("/robot/limb/right/joint_command",
JointCommand,
queue_size=100)
rospy.init_node('vel_test')
rate = rospy.Rate(1000)
i = 0
while not rospy.is_shutdown():
jointCmd = JointCommand()
jointCmd.mode = JointCommand.VELOCITY_MODE
jointCmd.names.append('right_s0')
jointCmd.names.append('right_s1')
jointCmd.names.append('right_e0')
jointCmd.names.append('right_e1')
jointCmd.names.append('right_w0')
jointCmd.names.append('right_w1')
jointCmd.names.append('right_w2')
if i < 4000:
vel = i / 1000.0
jointCmd.command.append(vel)
else:
jointCmd.command.append(0.0)
i = i + 1
jointCmdPub.publish(jointCmd)
rospy.loginfo("%s", jointCmd)
rospy.set_param('time', i)
rate.sleep()
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 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 followTrajectoryFromJointVelocity(self,
trajectory,
stop=True,
limb='left'):
if limb == 'left':
joint_names = self._left_limb.joint_names()
else:
joint_names = self._right_limb.joint_names()
topic = 'robot/limb/%s/joint_command' % limb
pub = rospy.Publisher(topic, JointCommand, queue_size=10)
rate = rospy.Rate(50.)
VELOCITY_MODE = 2
template = JointCommand()
template.mode = VELOCITY_MODE
template.names.extend(joint_names)
for pair in trajectory:
print pair
velocities, delta = pair
msg = copy.deepcopy(template)
msg.command.extend(velocities)
pub.publish(msg)
rospy.sleep(delta)
if stop:
velocities = [0.0] * len(joint_names)
cmd = dict(zip(joint_names, velocities))
if limb == 'left':
self._left_limb.set_joint_velocities(cmd)
else:
self._right_limb.set_joint_velocities(cmd)
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 vel_test(q_f_dot, t_dur, t_spent):
global CONST_FLAG
global STOP_FLAG
jointCmdPub = rospy.Publisher("/robot/limb/right/joint_command", JointCommand, queue_size = 100)
# rospy.init_node('vel_test')
rate_val = 100
rate = rospy.Rate(rate_val)
i = 0
print("\n")
print(t_dur)
print(t_spent)
t_dur_sec = float(t_dur.data.secs) + float(t_dur.data.nsecs)/np.power(10,9)
t = float(t_spent)
print("----------------")
print(t_dur_sec)
print(t_spent)
print(t)
print("\n")
STOP_FLAG = 0
while not rospy.is_shutdown() and not STOP_FLAG:
jointCmd = JointCommand()
jointCmd.mode = JointCommand.VELOCITY_MODE
# print("\n\n\n")
# print(t_dur)
# print(t_spent)
# t_dur_sec = float(t_dur.data.secs) + float(t_dur.data.nsecs)/np.power(10,9)
# t = float((t_dur_sec - t_spent))
# print("----------------")
# print(t_dur_sec)
# print(t_spent)
# print(t)
# print(t + (1/rate_val)
# print("\n\n\n")
t = float(t) + (1.0/rate_val)
# print(t)
# publish vels until we are past the duration we set
if t < t_dur_sec:
for i in range(len(q_f_dot)):
jointCmd.names.append(TRAJ_COM.names[i])
jointCmd.command.append(q_f_dot[i])
else:
STOP_FLAG = 1
CONST_FLAG = 0;
for i in range(len(q_f_dot)):
jointCmd.names.append(TRAJ_COM.names[i])
jointCmd.command.append(0)
rospy.loginfo("STOP FLAG WAS SET, DURATION WAS UP!")
# TrajectoryComamnd. = Duration
jointCmdPub.publish(jointCmd)
# rospy.loginfo("%s",jointCmd)
# rospy.loginfo(q_f_dot)
rate.sleep()
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 pubTruePos(pos_f, pub):
jointCmd = JointCommand()
jointCmd.mode = JointCommand.POSITION_MODE
for i in range(len(pos_f)):
jointCmd.names.append(TRAJ_CMD.names[i])
jointCmd.command.append(pos_f[i])
# publish message
pub.publish(jointCmd)
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 __init__(self):
# find out the absolute path of model file
fname = rospy.get_param('~file')
# create publisher and message instances
self.leftCommand = JointCommand()
self.rightCommand = JointCommand()
self.leftPub = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand, tcp_nodelay=True, queue_size=1)
self.rightPub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand, tcp_nodelay=True, queue_size=1)
# load bgplvm model from pickle file
self.model = pickle.load(open(fname,'rb'))
# parameters for doing latent function inference
self.qDim = self.model.X.mean.shape[1]
# create a joint_states publisher
self.statePub = rospy.Publisher('joint_states', JointState, latch=True,
queue_size=3)
# variables for generating motion
self.xMove = 0.0
self.yMove = 0.0
self.startMotion = False
# visualize the bgplvm latent space
self.maxPoints = MAXPOINTS
self.plotVariance = True
scales = self.model.kern.input_sensitivity(summarize=False)
self.plotIndices = np.argsort(scales)[-2:]
self.title = 'Latent Space Visualization'
# variables for real-time plotting
self.testHandle = None
self.pointerColor = GREEN
self.pointerHandle = None
self.testData = np.empty((0,2))
self.ax = self.plotLatent()
xmin, xmax = self.ax.get_xlim()
ymin, ymax = self.ax.get_ylim()
self.textHandle = plt.text(xmax/2, ymin+0.3, 'Play Mode: OFF',
bbox={'facecolor':'green', 'alpha':0.5, 'pad':10})
# connect the cursor class
plt.connect('button_press_event',self.mouseClick)
plt.connect('motion_notify_event', self.mouseMove)
plt.show()
raw_input('Press enter to finish')
return
def moveVelocity(self,
joint_values_target,
speed,
thresh,
is_printing=False):
"""
Velocity controller for Baxter arm movements.
"""
max_iterations = 5000
step_size = 2.0
for i in range(0, max_iterations):
joint_error = numpy.array(self.joint_values)
joint_velocity_command = joint_error
# calculate joint error, error magnitude, and command velocity
joint_error = joint_values_target - joint_error
joint_velocity_command = joint_error * step_size
error_magnitude = numpy.linalg.norm(joint_error)
joint_velocity_command_magnitude = error_magnitude * step_size
# if command velocity magnitude exceeds speed, scale it back to speed
if joint_velocity_command_magnitude > speed:
joint_velocity_command = speed * joint_velocity_command / joint_velocity_command_magnitude
# send velocity command to joints
values_out = [
joint_velocity_command[2], joint_velocity_command[3],
joint_velocity_command[0], joint_velocity_command[1],
joint_velocity_command[4], joint_velocity_command[5],
joint_velocity_command[6]
]
self.pub.publish(
JointCommand(JointCommand.VELOCITY_MODE, values_out,
self.joint_names))
if is_printing:
print "iteration:", i, "error_magnitude: ", error_magnitude
# break loop if close enough to target joint values
if error_magnitude < thresh:
break
rospy.sleep(0.001)
# need to reset to position mode; o.w. the robot will disable itself
values_out = [
joint_values_target[2], joint_values_target[3],
joint_values_target[0], joint_values_target[1],
joint_values_target[4], joint_values_target[5],
joint_values_target[6]
]
self.pub.publish(
JointCommand(JointCommand.POSITION_MODE, values_out,
self.joint_names))
def pubVels(q_f_dot, pub):
# set up joint command msg
jointCmd = JointCommand()
jointCmd.mode = JointCommand.VELOCITY_MODE
for i in range(len(q_f_dot)):
jointCmd.names.append(TRAJ_CMD.names[i])
jointCmd.command.append(q_f_dot[i])
# publish message
pub.publish(jointCmd)
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 followTrajectory(self, traj, startSpeed=0.75, endSpeed=0.25, startThresh=0.30, \
endThresh=0.10, forceThresh=float('inf')):
'''TODO'''
if not self.isMoving:
return
for i in xrange(len(traj)):
if i == len(traj) - 1:
self.moveVelocity(traj[i],
endSpeed,
endThresh,
forceThresh=forceThresh)
if i == len(traj) - 2:
self.moveVelocity(traj[i],
0.5 * startSpeed + 0.5 * endSpeed,
startThresh,
forceThresh=forceThresh)
else:
self.moveVelocity(traj[i],
startSpeed,
startThresh,
forceThresh=forceThresh)
# send a zero velocity command at the end of the trajectory
self.pub.publish(
JointCommand(JointCommand.VELOCITY_MODE, [0] * 7,
self.joint_names))
def __init__(self, limb):
"""
Constructor.
@type limb: str
@param limb: limb to interface
"""
self.name = limb
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._cartesian_pose = dict()
self._cartesian_velocity = dict()
self._cartesian_effort = dict()
self._joint_names = {
'left': [
'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0',
'left_w1', 'left_w2'
],
'right': [
'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0',
'right_w1', 'right_w2'
]
}
ns = '/robot/limb/' + limb + '/'
self._command_msg = JointCommand()
self._pub_speed_ratio = rospy.Publisher(ns + 'set_speed_ratio',
Float64,
latch=True)
self._pub_joint_cmd = rospy.Publisher(ns + 'joint_command',
JointCommand,
tcp_nodelay=True)
self._pub_joint_cmd_timeout = rospy.Publisher(ns +
'joint_command_timeout',
Float64,
latch=True)
_cartesian_state_sub = rospy.Subscriber(ns + 'endpoint_state',
EndpointState,
self._on_endpoint_states,
queue_size=1,
tcp_nodelay=True)
joint_state_topic = 'robot/joint_states'
_joint_state_sub = rospy.Subscriber(joint_state_topic,
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
err_msg = ("%s limb init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
baxter_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
timeout_msg=err_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 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 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 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 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 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 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 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 __init__(self, input_topics, output_topic):
limb = 'left'
for topic in input_topics:
rospy.Subscriber(topic,
std_msgs.msg.Float64,
callback=self.message_receive,
callback_args=topic)
self.names = ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']
self.msg = JointCommand()
self.msg.mode = JointCommand.TORQUE_MODE
self.msg.names = list()
for name in self.names:
self.msg.names.append(limb + '_' + name)
self.msg.command = [0] * 7
self.pub = rospy.Publisher(output_topic, JointCommand, queue_size=1)
self.rate = rospy.Rate(100)
self.publish_loop()
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 __init__(self, model_file, max_points, timer_freq, dim1, dim2,
resolution, manual):
# create joint angle commands and corresponding publishers
self.left_command = JointCommand(names=LEFT_NAMES,
mode=JointCommand.POSITION_MODE)
self.right_command = JointCommand(names=RIGHT_NAMES,
mode=JointCommand.POSITION_MODE)
self.left_pub = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand,
tcp_nodelay=True,
queue_size=1)
self.right_pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
tcp_nodelay=True,
queue_size=1)
# load mrd model from pickle file
self.mrd_model = GPy.load(model_file)
mrd_X = self.mrd_model.X.mean
self.mrd_point_count = mrd_X.shape[0]
if self.mrd_point_count > max_points:
rospy.logwarn('Mean contains more samples. Shape: (%d, %d)' %
mrd_X.shape)
downsample_indices = np.random.choice(self.mrd_point_count,
size=max_points,
replace=False)
mrd_X = mrd_X[downsample_indices]
# parameters for doing latent function inference
self.q_dim = mrd_X.shape[1]
self.latent_X = np.zeros((1, self.q_dim))
self.dim1 = dim1
self.dim2 = dim2
self.resolution = resolution
self.mrd_X = mrd_X[:, [self.dim1, self.dim2]]
title = 'Baxter Whill Movement using MRD'
fig, (self.ax1, self.ax2) = plt.subplots(nrows=1,
ncols=2,
figsize=(10, 4))
self.plot_latent_space()
self.plot_whill_movement()
fig.canvas.set_window_title(title)
self.text_handle = self.ax1.text(0.8,
0.1,
'Play Mode: OFF',
horizontalalignment='center',
verticalalignment='center',
transform=self.ax1.transAxes,
bbox={
'facecolor': 'green',
'alpha': 0.5,
'pad': 6
})
self.counter = 0
self.whill_move_handle = None
self.latent_cursor_handle = None
if manual:
fig.suptitle(
'Predicted Whill movements are not sent to the Whill controller',
fontstyle='italic',
color='red')
# variables for mouse cursor based motion
self.mouse_xy = np.zeros((1, 2))
self.start_motion = False
self.cursor_color = 'red'
# connect the cursor class
fig.canvas.mpl_connect('button_press_event', self.mouse_click)
fig.canvas.mpl_connect('motion_notify_event', self.mouse_move)
fig.subplots_adjust(top=0.80)
else:
self.whill_move = None
self.init_ros_whillpy()
self.cursor_color = 'green'
self.text_handle.set_text('Automatic Mode: ON')
# create a timer to follow the mean trajectory
self.ros_timer = rospy.Timer(rospy.Duration(1 / timer_freq),
self.timer_callback)
# adjust the space at the bottom
fig.subplots_adjust(bottom=0.15)
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 setJoints(self, angles, mode=1):
self.mode = mode
self.goalc = Point(*self.bk.forward_position_kinematics_arg2p(angles))
names = [name for name in iter(angles)]
angles = [angles[name] for name in iter(angles)]
self.jointCmd = JointCommand(mode, angles, names)
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_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)
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 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
