def callback(self, msg):
pub_msg = FollowJointTrajectoryActionGoal()
pub_msg.header = msg.header
pub_msg.goal_id = msg.goal_id
pub_msg.goal.trajectory.header = msg.goal.trajectory.header
pub_msg.goal.trajectory.joint_names = [
'r_thumb_roll', 'r_thumb_pitch', 'r_middle_pitch'
]
thumb_r_id = msg.goal.trajectory.joint_names.index('THUMB_R')
thumb_p_id = msg.goal.trajectory.joint_names.index('THUMB_P')
middle_p_id = msg.goal.trajectory.joint_names.index('MIDDLE_P')
pub_msg.goal.trajectory.points = []
for p in msg.goal.trajectory.points:
point = JointTrajectoryPoint()
point.positions = [
p.positions[thumb_r_id], p.positions[thumb_p_id],
p.positions[middle_p_id]
]
point.velocities = [
p.velocities[thumb_r_id], p.velocities[thumb_p_id],
p.velocities[middle_p_id]
]
point.time_from_start = p.time_from_start
pub_msg.goal.trajectory.points.append(point)
self.joint_command_pub.publish(pub_msg)
def goDownInShelf(self, x, y, z, alpha, beta, gamma):
traj = JointTrajectory()
traj.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
now = rospy.get_rostime()
rospy.loginfo("Current time %i %i", now.secs, now.nsecs)
traj.header.stamp = now
ag = FollowJointTrajectoryActionGoal()
ag.goal.trajectory = traj
touching_item = JointTrajectoryPoint()
touching_item.positions = [
math.radians(56.28),
math.radians(-82.90),
math.radians(99.17),
math.radians(-107.89),
math.radians(271.42),
math.radians(-38.20)
]
touching_item.velocities = self.default_velocity
touching_item.time_from_start = rospy.Duration(10.0)
traj.points.append(touching_item)
ag = FollowJointTrajectoryActionGoal()
ag.goal.trajectory = traj
self.joint_angle_pub.publish(ag)
rospy.sleep(10)
def publish_msg(self):
# Set the message to publish as command.
traj_vector = FollowJointTrajectoryActionGoal()
# Current ROS time stamp
h = Header()
h.stamp = rospy.Time.now()
traj_vector.header = h
traj_vector.goal.trajectory.joint_names.append('arm_1_joint')
traj_vector.goal.trajectory.joint_names.append('arm_2_joint')
traj_vector.goal.trajectory.joint_names.append('arm_3_joint')
traj_vector.goal.trajectory.joint_names.append('arm_4_joint')
traj_vector.goal.trajectory.joint_names.append('arm_5_joint')
traj_vector.goal.trajectory.joint_names.append('arm_6_joint')
traj_vector.goal_id.stamp = h.stamp
traj_vector.goal_id.id = 'rosPathPlanner' + str(h.stamp.secs)
h2 = Header()
h2.stamp.secs = h.stamp.secs + 1
h2.stamp.nsecs = h.stamp.nsecs + 3e8
traj_vector.goal.trajectory.header.stamp = h2.stamp
points2pub = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]
iter = 0.0
for column in points2pub:
point = JointTrajectoryPoint()
iter += 1
for q in column:
point.positions.append(q)
point.time_from_start.nsecs = 0
point.time_from_start.secs = iter * 10
traj_vector.goal.trajectory.points.append(point)
print traj_vector
print 'All systems go!'
self.trajPub.publish(traj_vector)
def goUpInShelf(self, x, y, z, alpha, beta, gamma):
traj = JointTrajectory()
traj.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
now = rospy.get_rostime()
rospy.loginfo("Current time %i %i", now.secs, now.nsecs)
traj.header.stamp = now
ag = FollowJointTrajectoryActionGoal()
ag.goal.trajectory = traj
inside = JointTrajectoryPoint()
inside.positions = [
math.radians(56.51),
math.radians(-85.19),
math.radians(95.29),
math.radians(-100.09),
math.radians(270.91),
math.radians(-38.14)
]
inside.velocities = self.default_velocity
inside.time_from_start = rospy.Duration(10.0)
traj.points.append(inside)
ag = FollowJointTrajectoryActionGoal()
ag.goal.trajectory = traj
self.joint_angle_pub.publish(ag)
rospy.sleep(10)
def data_loop(self, data):
target_angles = eval(data.decode())
trajectory_msg = self.generate_trajectory_msg(target_angles)
goal_msg = FollowJointTrajectoryGoal()
goal_msg.trajectory = trajectory_msg
action_goal_msg = FollowJointTrajectoryActionGoal()
action_goal_msg.header = trajectory_msg.header
action_goal_msg.goal = goal_msg
self.pub.publish(action_goal_msg)
def gotoBin(self, binName):
traj = JointTrajectory()
traj.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
bin = JointTrajectoryPoint()
bin.velocities = self.default_velocity
bin.time_from_start = rospy.Duration(5.0)
if (binName == "A"):
rospy.loginfo("Going to Shelf A!")
bin.positions = self.shelfA_position
elif (binName == "B"):
rospy.loginfo("Going to Shelf B")
bin.positions = self.shelfB_position
elif (binName == "C"):
rospy.loginfo("Going to Shelf C")
bin.positions = self.shelfC_position
elif (binName == "D"):
rospy.loginfo("Going to Shelf D")
bin.positions = self.shelfD_position
elif (binName == "E"):
rospy.loginfo("Going to Shelf E")
bin.positions = self.shelfE_position
elif (binName == "F"):
rospy.loginfo("Going to Shelf F")
bin.positions = self.shelfF_position
elif (binName == "G"):
rospy.loginfo("Going to Shelf G")
bin.positions = self.shelfG_position
elif (binName == "H"):
rospy.loginfo("Going to Shelf H")
bin.positions = self.shelfH_position
elif (binName == "I"):
rospy.loginfo("Going to Shelf I")
bin.positions = self.shelfI_position
elif (binName == "J"):
rospy.loginfo("Going to Shelf J")
bin.positions = self.shelfJ_position
elif (binName == "K"):
rospy.loginfo("Going to Shelf K")
bin.positions = self.shelfK_position
elif (binName == "L"):
rospy.loginfo("Going to Shelf L")
bin.positions = self.shelfL_position
else:
bin.positions = self.home_position
traj.points.append(bin)
now = rospy.get_rostime()
rospy.loginfo("Current time %i %i", now.secs, now.nsecs)
traj.header.stamp = now
ag = FollowJointTrajectoryActionGoal()
ag.goal.trajectory = traj
self.joint_angle_pub.publish(ag)
rospy.sleep(5)
def talker():
rospy.loginfo('starting the talker node')
global counter
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
pub = rospy.Publisher('/arm_controller/follow_joint_trajectory/goal',
FollowJointTrajectoryActionGoal,
queue_size=10)
rospy.sleep(2)
command = FollowJointTrajectoryActionGoal()
command.header.stamp = rospy.Time.now()
command.goal.trajectory.joint_names = ['elbow']
point = JointTrajectoryPoint()
point.positions = [math.pi / 2]
point.velocities = [1.5] #0.5
#point.positions = [math.sin(counter/4)*math.pi]
# in which range is sinus defined?
point.time_from_start = rospy.Duration(10)
command.goal.trajectory.points.append(point)
pub.publish(command)
rospy.loginfo('=====send command %r', command.goal.trajectory.points[0])
rospy.sleep(2)
counter += 1
rate.sleep()
def execute_trajectory(self, traj):
if self._execution_running:
rospy.logwarn(
'Request to execute a trajectory while an old one is already running'
)
self._action_client.cancel_goal()
self._execution_running = False
traj.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.1)
self._current_arm_traj = ROSUtils.extract_part_trajectory(
self._arm_joint_names, traj)
self._current_hand_traj = ROSUtils.extract_part_trajectory(
self._hand_joint_names, traj)
self._current_traj = traj
arm_traj_goal = FollowJointTrajectoryActionGoal()
arm_traj_goal.goal.trajectory = self._current_arm_traj
arm_traj_goal.goal.goal_tolerance = self._goal_tolerance
arm_traj_goal.goal.path_tolerance = self._path_tolerance
# TODO for some reason we do not receive feedback, so we add a fake feedback caller
# TODO here
self._action_client.send_goal(
arm_traj_goal.goal,
active_cb=self._receive_arm_traj_start_signal,
feedback_cb=self._receive_arm_traj_feedback,
done_cb=self._receive_arm_traj_finish_signal)
self._fake_feedback_timer = rospy.Timer(rospy.Duration.from_sec(0.05),
self._simulate_feedback,
oneshot=False)
# TODO return whether we grasped an object
return True
def gotoxy_handle(req):
move = FollowJointTrajectoryActionGoal()
x= req.x
y = req.y#float(sys.argv[1])
offset = 0.048
X= y - offset
Y= -x
l1= 0.08
l2= 0.047
theta2 = math.acos((X*X + Y*Y - l1*l1 - l2*l2)/(2*l1*l2))
alpha = math.atan2(Y,X)
beta = math.acos((X*X + Y*Y + l1*l1 - l2*l2)/(2*l1*math.sqrt(X*X + Y*Y)))
theta1 = alpha - beta
point = JointTrajectoryPoint()
point.positions = [theta1,-theta2]
point.time_from_start = rospy.Duration(3)
move.goal.trajectory.joint_names = ['shoulder_1_joint', 'shoulder_2_joint']
move.goal.trajectory.points.append(point)
client.send_goal(move.goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
return True
def cmdJointState(self, joint_pos, time=5.0):
'''
To command a joint position to the hand..
'''
self.switch_operation_mode('position')
rospy.sleep(0.02)
if self.joint_names is not None:
point = JointTrajectoryPoint()
point.positions = joint_pos
point.time_from_start.secs = time
traj = JointTrajectory()
traj.header.stamp = rospy.Time.now()
traj.joint_names = self.joint_names
traj.points.append(point)
action_goal = FollowJointTrajectoryActionGoal()
action_goal.header.stamp = rospy.Time.now()
action_goal.goal.trajectory = traj
action_goal.goal.goal_time_tolerance.secs = time
self.cmd_joint_state_pub.publish(action_goal)
rospy.sleep(time)
self.switch_operation_mode('velocity')
rospy.sleep(0.02)
def move_joint_traj(self, traj, dt):
goal = FollowJointTrajectoryActionGoal()
goal.goal.trajectory.joint_names = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6',
'joint_7', 'joint_8', 'joint_9', 'joint_a'
]
for i in range(len(traj)):
point = JointTrajectoryPoint()
p = [0] * self.joint_num
v = [0] * self.joint_num
last_v = [0] * self.joint_num
a = [0] * self.joint_num
if i != len(traj) - 1 and i != 0:
for j in range(self.joint_num):
p[j] = traj[i][j]
v[j] = (traj[i + 1][j] - traj[i][j]) / dt
last_v[j] = (traj[i][j] - traj[i - 1][j]) / dt
a[j] = (v[j] - last_v[j]) / dt
if i == len(traj) - 1:
p = traj[-1]
elif i == 0:
p = traj[0]
point.positions = p
point.velocities = v
point.accelerations = a
point.time_from_start = rospy.Duration((i + 1) * dt)
goal.goal.trajectory.points.append(point)
self.jta.send_goal_and_wait(goal.goal)
def main():
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node("ik_moveit")
pub = rospy.Publisher('/arm_controller/follow_joint_trajectory/goal',
FollowJointTrajectoryActionGoal,
queue_size=1)
compute_ik_srv = rospy.ServiceProxy("/compute_ik", GetPositionIK)
rospy.wait_for_service("/compute_ik")
arm_move_group = moveit_commander.MoveGroupCommander("manipulator")
robot_commander = moveit_commander.RobotCommander()
# compute reference position of the gripper ( link)
ref_pose = pq_to_pose([0.256, 0.294, 0.365], [0.707, 0.707, 0.0, 0.0])
rate = rospy.Rate(10.0)
iter_no = 0
max_iter = 10
while not rospy.is_shutdown():
# set reference position of the gripper ( link)
pose_ik = PoseStamped()
pose_ik.header.frame_id = "base_link"
pose_ik.header.stamp = rospy.Time.now()
pose_ik.pose = ref_pose
req = GetPositionIKRequest()
req.ik_request.group_name = "manipulator"
req.ik_request.robot_state = robot_commander.get_current_state()
req.ik_request.avoid_collisions = True
req.ik_request.ik_link_name = arm_move_group.get_end_effector_link()
req.ik_request.pose_stamped = pose_ik
# print(arm_move_group.get_end_effector_link())
ik_response = compute_ik_srv(req)
if ik_response.error_code.val == 1:
print('Goal state:')
print(ik_response.solution.joint_state.position)
goal_pose = FollowJointTrajectoryActionGoal()
# print (arm_move_group.get_joints())
goal_pose.goal.trajectory.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
goal_pose.goal.trajectory.points.append(JointTrajectoryPoint())
goal_pose.goal.trajectory.points[
0].positions = ik_response.solution.joint_state.position
goal_pose.goal.trajectory.points[0].velocities = [0, 0, 0, 0, 0, 0]
goal_pose.goal.trajectory.points[
0].time_from_start = rospy.Duration.from_sec(1.0)
pub.publish(goal_pose)
else:
print('Could not find solution to inverse kinematic')
rate.sleep()
iter_no = iter_no + 1
if iter_no > max_iter:
break
def arm_hand_action_control(self, q_goal):
# JointTrajectory
joint_goal = self.set_arm_hand_joint_goal(q_goal)
# FollowJointTrajectoryActionGoal
action_goal = FollowJointTrajectoryActionGoal()
action_goal.goal.trajectory = joint_goal
# send goal
self.client.send_goal(action_goal.goal)
def move(self, pos):
msg = FollowJointTrajectoryActionGoal()
msg.goal.trajectory.joint_names = self.joint_name_lst
point = JointTrajectoryPoint()
point.positions = pos
point.time_from_start = rospy.Duration(1.0 / 60.0)
msg.goal.trajectory.points.append(point)
self.jta.send_goal_and_wait(msg.goal)
def command():
if len(sys.argv) != 7:
print "Usage: ", sys.argv[
0], "<angle_joint_1> <angle_joint_2> <angle_joint_3> <angle_joint_4> <angle_joint_5> <angle_joint_6>"
return -1
else:
global flag
global actual_config
flag = False
rospy.init_node('configuration_commander', anonymous=True)
# rospy.loginfo("SERVER")
rospy.Subscriber("/joint_states", JointState, state_callback)
pub = rospy.Publisher('/joint_trajectory_action/goal',
FollowJointTrajectoryActionGoal,
queue_size=10)
rospy.sleep(1)
if flag:
# Starting configuration initialization based on the joint state stored by state_callback
start_config = JointTrajectoryPoint()
start_config.positions = actual_config
start_config.time_from_start.secs = 0
start_config.time_from_start.nsecs = 0
# Goal configuration initialization
goal_config = JointTrajectoryPoint()
goal_config_array = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
for i in range(0, 6):
goal_config_array[i] = float(sys.argv[i + 1])
print goal_config_array[i]
goal_config.positions = goal_config_array
goal_config.time_from_start.secs = 1
goal_config.time_from_start.nsecs = 63310604
# Message inizialization
traj_msg = FollowJointTrajectoryActionGoal()
traj_msg.goal.trajectory.header.seq = 0
traj_msg.goal.trajectory.header.stamp.nsecs = 0
traj_msg.goal.trajectory.header.stamp.secs = 0
traj_msg.goal.trajectory.header.frame_id = 'base_link'
traj_msg.goal.trajectory.joint_names = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5',
'joint_6'
]
traj_msg.goal.trajectory.points = [start_config, goal_config]
traj_msg.goal.goal_time_tolerance.secs = 0
traj_msg.goal.goal_time_tolerance.nsecs = 0
# rospy.loginfo(goal_config.positions[1])
# Message pubblication on "/joint_trajectory_action/goal"
pub.publish(traj_msg)
flag = False
else:
pass
def gotoBox(self):
# hardcoded for now
traj = JointTrajectory()
traj.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
now = rospy.get_rostime()
rospy.loginfo("Current time %i %i", now.secs, now.nsecs)
traj.header.stamp = now
rospy.loginfo("Going to Box")
home = JointTrajectoryPoint()
home.positions = self.home_position
home.velocities = self.default_velocity
home.time_from_start = rospy.Duration(10.0)
traj.points.append(home)
Bin_way1 = JointTrajectoryPoint()
Bin_way1.positions = [
math.radians(4.78), self.home_position[1], self.home_position[2],
self.home_position[3], self.home_position[4], self.home_position[5]
]
Bin_way1.velocities = self.default_velocity
Bin_way1.time_from_start = rospy.Duration(15.0)
traj.points.append(Bin_way1)
Bin_way2 = JointTrajectoryPoint()
Bin_way2.positions = [
math.radians(4.78),
math.radians(-78.95),
math.radians(142.9),
math.radians(-154.65),
math.radians(271.99),
math.radians(2.51)
]
Bin_way2.velocities = self.default_velocity
Bin_way2.time_from_start = rospy.Duration(20.0)
traj.points.append(Bin_way2)
Bin = JointTrajectoryPoint()
Bin.positions = [
math.radians(33.62),
math.radians(-35.03),
math.radians(125.73),
math.radians(-181.48),
math.radians(275.56),
math.radians(0.76)
]
Bin.velocities = self.default_velocity
Bin.time_from_start = rospy.Duration(25.0)
traj.points.append(Bin)
ag = FollowJointTrajectoryActionGoal()
ag.goal.trajectory = traj
self.joint_angle_pub.publish(ag)
rospy.sleep(25)
def move(self, pos):
msg = FollowJointTrajectoryActionGoal()
msg.goal.trajectory.joint_names = self.joint_names
point = JointTrajectoryPoint()
point.positions = pos
point.time_from_start = rospy.Duration(1.0 / 60.0)
msg.goal.trajectory.points.append(point)
self.action_server_client.send_goal(msg.goal)
return True
def reset_move(self, pos):
jtp_msg = JointTrajectory()
self.jtp.publish(jtp_msg)
msg = FollowJointTrajectoryActionGoal()
msg.goal.trajectory.joint_names = self.joint_name_lst
point = JointTrajectoryPoint()
point.positions = pos
point.time_from_start = rospy.Duration(0.0001)
msg.goal.trajectory.points.append(point)
self.jta.send_goal(msg.goal)
def __init__(self):
self.trajPub = rospy.Publisher(
'/arm_controller/follow_joint_trajectory/goal',
FollowJointTrajectoryActionGoal,
queue_size=10)
self.pointPub = rospy.Publisher('/arm_controller/command',
JointTrajectory,
queue_size=10)
self.msg2pub = FollowJointTrajectoryActionGoal()
self.msg2 = JointTrajectory()
def arm_action_control(self, q_goal):
print("== Waiting for action server... ")
self.arm_client.wait_for_server()
# JointTrajectory
joint_goal = self.set_arm_joint_goal(q_goal)
# FollowJointTrajectoryActionGoal
action_goal = FollowJointTrajectoryActionGoal()
action_goal.goal.trajectory = joint_goal
# send goal
self.arm_client.send_goal(action_goal.goal)
self.arm_client.wait_for_result()
def arm_hand_action_control(self, q_arm_goal, q_hand_goal):
print("== Waiting for action server... ")
self.arm_client.wait_for_server()
self.hand_client.wait_for_server()
# JointTrajectory
arm_joint_goal = self.set_arm_joint_goal(q_arm_goal)
hand_joint_goal = self.set_arm_joint_goal(q_hand_goal)
# FollowJointTrajectoryActionGoal
arm_action_goal = FollowJointTrajectoryActionGoal()
arm_action_goal.goal.trajectory = arm_joint_goal
hand_action_goal = FollowJointTrajectoryActionGoal()
hand_action_goal.goal.trajectory = hand_joint_goal
# send goals
self.arm_client.send_goal(arm_action_goal.goal)
self.hand_client.send_goal(hand_action_goal.goal)
# wait for results
self.arm_client.wait_for_result()
self.hand_client.wait_for_result()
def move_robot(pose):
goal_pose = FollowJointTrajectoryActionGoal()
goal_pose.goal.trajectory.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
goal_pose.goal.trajectory.points.append(JointTrajectoryPoint())
goal_pose.goal.trajectory.points[0].positions = pose
goal_pose.goal.trajectory.points[0].velocities = [0, 0, 0, 0, 0, 0]
goal_pose.goal.trajectory.points[
0].time_from_start = rospy.Duration.from_sec(1.0)
return goal_pose
def callback(self, anymsg, topic_name):
if self.pause_button.isChecked():
return
# In case of control_msgs/FollowJointTrajectoryActionGoal
# set trajectory_msgs/JointTrajectory to 'msg'
# Convert AnyMsg to trajectory_msgs/JointTrajectory
msg_class = self.topic_name_class_map[topic_name]
if msg_class == JointTrajectory:
msg = JointTrajectory().deserialize(anymsg._buff)
elif msg_class == FollowJointTrajectoryActionGoal:
msg = FollowJointTrajectoryActionGoal().deserialize(
anymsg._buff).goal.trajectory
elif msg_class == DisplayTrajectory:
if DisplayTrajectory().deserialize(
anymsg._buff).trajectory.__len__() > 0:
msg = DisplayTrajectory().deserialize(
anymsg._buff).trajectory.pop().joint_trajectory
else:
rospy.logwarn(
"Received planned trajectory has no waypoints in it. Nothing to plot!"
)
return
else:
rospy.logerr('Wrong message type %s' % msg_class)
return
self.time = np.array([0.0] * len(msg.points))
(self.dis, self.vel, self.acc, self.eff) = ({}, {}, {}, {})
for joint_name in msg.joint_names:
self.dis[joint_name] = np.array([0.0] * len(msg.points))
self.vel[joint_name] = np.array([0.0] * len(msg.points))
self.acc[joint_name] = np.array([0.0] * len(msg.points))
self.eff[joint_name] = np.array([0.0] * len(msg.points))
for i in range(len(msg.points)):
point = msg.points[i]
self.time[i] = point.time_from_start.to_sec()
for j in range(len(msg.joint_names)):
joint_name = msg.joint_names[j]
if point.positions:
self.dis[joint_name][i] = point.positions[j]
if point.velocities:
self.vel[joint_name][i] = point.velocities[j]
if point.accelerations:
self.acc[joint_name][i] = point.accelerations[j]
if point.effort:
self.eff[joint_name][i] = point.effort[j]
if self.joint_names != msg.joint_names:
self.joint_names = msg.joint_names
self.refresh_tree()
self.joint_names = msg.joint_names
self.plot_graph()
def functional():
pub_gary = rospy.Publisher('/gary/joint_trajectory_action/goal',
FollowJointTrajectoryActionGoal,
queue_size=10)
pub_rosey = rospy.Publisher('/rosey/joint_trajectory_action/goal',
FollowJointTrajectoryActionGoal,
queue_size=10)
rospy.init_node('traj_maker', anonymous=True)
time.sleep(4)
rate = rospy.Rate(0.01)
while not rospy.is_shutdown():
traj_waypoint_1_gary = JointTrajectoryPoint()
traj_waypoint_1_rosey = JointTrajectoryPoint()
traj_waypoint_1_gary.positions = [0, 0, 0, 0, 0, 0]
traj_waypoint_1_rosey.positions = [0, 0, 0, 0, 0, 0]
traj_waypoint_1_rosey.time_from_start.secs = 10
traj_waypoint_1_gary.time_from_start.secs = 10
# making message
message_gary = FollowJointTrajectoryActionGoal()
message_rosey = FollowJointTrajectoryActionGoal()
# required headers
header_gary = std_msgs.msg.Header(stamp=rospy.Time.now())
header_rosey = std_msgs.msg.Header(stamp=rospy.Time.now())
message_gary.goal.trajectory.header = header_gary
message_rosey.goal.trajectory.header = header_rosey
message_gary.header = header_gary
message_rosey.header = header_rosey
# adding in joints
joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', \
'joint_5', 'joint_6']
message_gary.goal.trajectory.joint_names = joint_names
message_rosey.goal.trajectory.joint_names = joint_names
# appending up to 100 points
# ex. for i in enumerate(len(waypoints)): append(waypoints[i])
message_gary.goal.trajectory.points.append(traj_waypoint_1_gary)
message_rosey.goal.trajectory.points.append(traj_waypoint_1_rosey)
# publishing to ROS node
pub_gary.publish(message_gary)
pub_rosey.publish(message_rosey)
rate.sleep()
if rospy.is_shutdown():
break
break
def position_callback(self, msg):
action_goal = FollowJointTrajectoryActionGoal()
action_goal.goal_id.stamp = rospy.get_rostime()
action_goal.goal.trajectory.joint_names = self._joint_names
target_point = JointTrajectoryPoint()
if self._dof == len(msg.positions):
target_point.positions = msg.positions
else:
target_point.positions = []
for position in msg.positions:
target_point.positions.append(position)
for i in range(self._dof - len(msg.positions)):
target_point.positions.append(0.0)
target_point.velocities = [self._velocity] * self._dof
target_point.accelerations = [self._acceleration] * self._dof
action_goal.goal.trajectory.points.append(target_point)
self._pub.publish(action_goal)
def command():
global flag
global actual_config
flag = False
rospy.init_node('set_to_zero', anonymous=True)
rospy.Subscriber("/joint_states", JointState, state_callback)
pub = rospy.Publisher('/joint_trajectory_action/goal',
FollowJointTrajectoryActionGoal,
queue_size=10)
rospy.sleep(1)
if flag:
# Starting configuration initialization based on the joint state stored by state_callback
start_config = JointTrajectoryPoint()
start_config.positions = actual_config
start_config.time_from_start.secs = 0
start_config.time_from_start.nsecs = 0
# Goal configuration initialization: default position (all zeros)
goal_config = JointTrajectoryPoint()
goal_config.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
goal_config.time_from_start.secs = 1
goal_config.time_from_start.nsecs = 63310604
# Message inizialization
traj_msg = FollowJointTrajectoryActionGoal()
traj_msg.goal.trajectory.header.seq = 0
traj_msg.goal.trajectory.header.stamp.nsecs = 0
traj_msg.goal.trajectory.header.stamp.secs = 0
traj_msg.goal.trajectory.header.frame_id = 'base_link'
traj_msg.goal.trajectory.joint_names = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
]
traj_msg.goal.trajectory.points = [start_config, goal_config]
traj_msg.goal.goal_time_tolerance.secs = 0
traj_msg.goal.goal_time_tolerance.nsecs = 0
# rospy.loginfo(goal_config.positions[1])
# Message pubblication on "/joint_trajectory_action/goal"
pub.publish(traj_msg)
flag = False
else:
pass
def main():
# pub = rospy.Publisher('/if_any', String, queue_size=10)
traj_pub = rospy.Publisher('/arm_controller/follow_joint_trajectory/goal',
FollowJointTrajectoryActionGoal,
queue_size=10)
rospy.init_node('generate_trajectory', anonymous=True)
rospy.Subscriber("/joint_states", JointState, js_callback)
rospy.sleep(0.5)
while 'last_js' not in globals() and not rospy.is_shutdown():
pass
joint_limits = np.array([[-math.pi, math.pi]] * 6)
# joint_limits[0:2, :] = 0.
vel_limit = 1.0
acc = 10.0
time_step = 0.1
traj = gen_traj(joint_limits=joint_limits,
action_step=10,
duration=10,
acc_mod=10.,
vel_limit=0.5,
time_step=0.01)
goal = FollowJointTrajectoryActionGoal()
goal.goal.trajectory = traj
# goal.header.stamp = rospy.Time.now()
# goal.goal_id.stamp = rospy.Time.now()
# goal.goal_id.id = ''.join(random.choice(string.ascii_uppercase + string.digits) for _ in range(10))
# print(goal)
import rosbag
bag = rosbag.Bag('test.bag', 'w')
try:
bag.write('/follow_joint_trajectory/goal', goal)
finally:
bag.close()
traj_pub.publish(goal)
def make_trajectory_goal_msg(joint_trajectory_plan=[], seq=0, secs=0, nsecs=0, dt=2, frame_id='/base_link'):
'''
Converts a joint trajectory plan to a FollowJoinTrajectoryActionGoal message
which is compatible with the topic /arm_N/arm_controller/follow_joint_trajectory/goal
'''
# Goal ID Generator
id_gen = actionlib.GoalIDGenerator()
# create joint trajectory message
jtg = FollowJointTrajectoryActionGoal(goal_id=id_gen.generate_ID())
# fill the header
jtg.header.seq = seq
jtg.header.stamp.secs = 0 #secs
jtg.header.stamp.nsecs = 0 #nsecs
jtg.header.frame_id = frame_id
# specify the joint names
jtg.goal.trajectory.joint_names = joint_trajectory_plan.joint_trajectory.joint_names
# fill the trajectory points and computer constraint times
njtp = len(joint_trajectory_plan.joint_trajectory.points)
for ii in range(0, njtp):
jtp = JointTrajectoryPoint()
jtp = copy.deepcopy(joint_trajectory_plan.joint_trajectory.points[ii])
jtp.time_from_start = rospy.Duration().from_sec(secs + dt*(ii+1))
jtg.goal.trajectory.points.append(jtp)
'''jtg.goal.goal_tolerance = JointTolerance()
jtg.goal.goal_tolerance.name = "goaltol"
jtg.goal.goal_tolerance.position = 0.02
jtg.goal.goal_tolerance.velocity = 0.5
jtg.goal.goal_tolerance.acceleration = 0.5
jtg.goal.path_tolerance = JointTolerance()
jtg.goal.path_tolerance.name = "goaltol"
jtg.goal.path_tolerance.position = 0.02
jtg.goal.path_tolerance.velocity = 0.5
jtg.goal.path_tolerance.acceleration = 0.5
jtg.goal.goal_time_tolerance = rospy.Duration()
jtg.goal.goal_time_tolerance.set(5, 0)'''
return jtg
def test_callback(data_input):
global message
message = data_input.actual.positions
msg_list = list(message)
#msg_list[0] = int(message[0].encode('hex'),16)
#for i in
#msg_list = int(message.encode('hex'),16)
#print('============= Received image data.',message)
rospy.loginfo('=====received data %r', msg_list[0])
timer = Timer()
dt = 0.1
p.setup(timestep=dt) # 0.1ms
pub = rospy.Publisher('/arm_controller/follow_joint_trajectory/goal',
FollowJointTrajectoryActionGoal,
queue_size=10)
command = FollowJointTrajectoryActionGoal()
command.header.stamp = rospy.Time.now()
command.goal.trajectory.joint_names = ['elbow']
point = JointTrajectoryPoint()
point.positions = [rate_command / 10]
point.time_from_start = rospy.Duration(1)
command.goal.trajectory.points.append(point)
pub.publish(command)
rospy.loginfo('=====send command %r', command.goal.trajectory.points[0])
print("now plotting the network---------------")
rospy.loginfo('--------now plotting---------------')
n_panels = sum(a.shape[1]
for a in pop_1_data.segments[0].analogsignalarrays) + 2
plt.subplot(n_panels, 1, 1)
plot_spiketrains(pop_1_data.segments[0])
panel = 3
for array in pop_1_data.segments[0].analogsignalarrays:
for i in range(array.shape[1]):
plt.subplot(n_panels, 1, panel)
plot_signal(array, i, colour='bg'[panel % 2])
panel += 1
plt.xlabel("time (%s)" % array.times.units._dimensionality.string)
plt.setp(plt.gca().get_xticklabels(), visible=True) #
def move_robot(self, current_state):
if current_state in self.state_to_gripper_pose:
print(
f"Moving robot to the pose p={self.state_to_gripper_pose[current_state][0]},"
f" q={self.state_to_gripper_pose[current_state][1]}")
# set reference position of the gripper ( link)
pose_ik = PoseStamped()
pose_ik.header.frame_id = "base_link"
pose_ik.header.stamp = rospy.Time.now()
pose_ik.pose = self.pq_to_pose(
*self.state_to_gripper_pose[current_state])
req = GetPositionIKRequest()
req.ik_request.group_name = "manipulator"
req.ik_request.robot_state = self.robot_commander.get_current_state(
)
req.ik_request.avoid_collisions = True
req.ik_request.ik_link_name = self.arm_move_group.get_end_effector_link(
)
req.ik_request.pose_stamped = pose_ik
ik_response = self.compute_ik_srv(req)
if ik_response.error_code.val == 1:
goal_pose = FollowJointTrajectoryActionGoal()
# print (arm_move_group.get_joints())
goal_pose.goal.trajectory.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
goal_pose.goal.trajectory.points.append(JointTrajectoryPoint())
goal_pose.goal.trajectory.points[
0].positions = ik_response.solution.joint_state.position
goal_pose.goal.trajectory.points[0].velocities = [
0, 0, 0, 0, 0, 0
]
goal_pose.goal.trajectory.points[
0].time_from_start = rospy.Duration.from_sec(1.0)
self.pub.publish(goal_pose)
else:
print('Could not find solution to inverse kinematic')
def execute_cb(self, goal):
success = True
#create action for arm
arm_action = FollowJointTrajectoryAction()
#create two seperate goals, one for base and one for arm
base_fjtag = FollowJointTrajectoryActionGoal()
arm_fjtag = FollowJointTrajectoryActionGoal()
#set the base goal and name the virtual joints
base_goal = FollowJointTrajectoryGoal()
base_jt = JointTrajectory()
base_jt.joint_names = ["virtual_x", "virtual_y", "virtual_theta"]
#set the arm goal and name the arm joints
arm_goal = FollowJointTrajectoryGoal()
arm_jt = JointTrajectory()
arm_jt.joint_names = ["arm_joint_1", "arm_joint_2", "arm_joint_3","arm_joint_4","arm_joint_5"]
for pt in goal.trajectory.points:
# Fetch the base position from Moveit plan
pt_temp = JointTrajectoryPoint()
pt_temp.positions.append(pt.positions[6])
pt_temp.positions.append(pt.positions[7])
pt_temp.positions.append(pt.positions[5])
pt_temp.time_from_start = pt.time_from_start
base_jt.points.append(pt_temp)
pt_temp = JointTrajectoryPoint()
#allocate the last five pt.position(for arm) elements to arm trajectory
pt_temp.positions = pt.positions[0:5]
# pt_temp.velocities = pt.velocities[0:5]
# pt_temp.accelerations = pt.accelerations[0:5]
#
pt_temp.time_from_start = pt.time_from_start + rospy.Duration(0.1)
arm_jt.points.append(pt_temp)
base_goal.trajectory = base_jt
arm_goal.trajectory = arm_jt
base_fjtag.goal = base_goal
arm_fjtag.goal = arm_goal
arm_action = arm_goal
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo("This action has been preempted")
self._as.set_preempted()
success = False
self._as.set_cancelled()
#if preempted, can do something here
else:
self.pub1.publish(base_fjtag)
self.arm_client.send_goal_and_wait(arm_action)
success = True
# self.pub2.publish(arm_fjtag)
if success:
rospy.loginfo('Action Succeeded')
self._as.set_succeeded()