def shake_head(self, effort=1.0):
point = None
traj = JointTrajectory()
traj.joint_names = [self.head.JOINT_PAN, self.head.JOINT_TILT]
cur_head_pos = self.head.get_head_pos()
cur_pan = cur_head_pos[0]
cur_tilt = cur_head_pos[1]
effort = [effort, effort]
for i in range(1, 4):
point = JointTrajectoryPoint()
if i % 2:
# move head left
point.positions = [-0.3, cur_tilt]
point.time_from_start.nsecs = 0.5 * 1e9
else:
# move head right
point.positions = [0.3, cur_tilt]
point.time_from_start.secs = i / 2
point.effort = effort
traj.points.append(point)
# go back to original position
reset_point = JointTrajectoryPoint()
reset_point.positions = [cur_pan, cur_tilt]
reset_point.effort = effort
reset_point.time_from_start = 3 # note this is relative to max of loop range
traj.points.append(reset_point)
# set all red lights, send traj, reset lights
self.lights.all_leds(self.lights.RED)
self.head.send_trajectory(traj=traj,
feedback_cb=self._feedback_cb,
done_cb=self._done_cb)
self.head.wait_for_done(5)
self.lights.off()
def callback(self, data):
if data.buttons[1] == 1:
grip = GripperApplyEffortActionGoal()
grip.goal.effort = -0.05
print "grasp"
# self.grip_act.publish(grip)
pp = JointTrajectoryPoint()
pp.positions = [-0.05, -0.05]
pp.velocities = []
pp.effort = []
self.hand_sig.points = [pp]
self.pub_sig.publish(self.hand_sig)
time.sleep(0.1)
self.hand_flug = False
## 改良
elif data.buttons[2] == 1:
print "open"
# self.pub_list.publish(self.hand_traj)
pp = JointTrajectoryPoint()
pp.positions = [0.61, -0.61]
pp.velocities = []
pp.effort = []
self.hand_sig.points = [pp]
self.pub_sig.publish(self.hand_sig)
time.sleep(0.1)
self.hand_flug = True
def callback(self, data):
if data.buttons[1] == 1:
grip = GripperApplyEffortActionGoal()
grip.goal.effort = -0.02
print "grasp"
self.grip_act.publish(grip)
pp = JointTrajectoryPoint()
pp.positions = [-0.05, -0.05]
pp.velocities = []
pp.effort = []
self.hand_sig.points = [pp]
self.pub_sig.publish(self.hand_sig)
time.sleep(0.1)
self.hand_flug = False
elif data.buttons[3] == 1:
self.flag_record = True
self.obj_name = ""
## 改良
elif data.buttons[2] == 1:
print "open"
self.pub_list.publish(self.hand_traj)
pp = JointTrajectoryPoint()
pp.positions = [0.61, -0.61]
pp.velocities = []
pp.effort = []
self.hand_sig.points = [pp]
self.pub_sig.publish(self.hand_sig)
time.sleep(0.1)
self.hand_flug = True
elif data.buttons[0] == 1:
self.obj_name = ""
if not self.record_flag and self.flag_record:
self.flag_record = False
bb_req = RosbagRecordRequest()
bb_req.node_name = "joint_teleope_bag"
bb_req.save_name = "exp_{0:s}_{1:d}".format(ENV, self.number)
bb_req.split_duration_str = "60"
bb_req.record_topic_list = TOPICS
self.rosbag_time = rospy.Time.now()
self.record_number = 0
self.record_flag = True
res = self.srv_record_start.call(bb_req)
elif self.flag_record and self.flag_record:
bs_req = RosbagStopRequest()
bs_req.rosbag_number = self.record_number
self.record_flag = False
self.number += 1
self.flag_record = False
self.srv_record_stop.call(bs_req)
rospy.sleep(1.0)
elif data.buttons[16] == 1:
st = EmptyRequest()
a = self.srv_marker.call(st)
def __init__(self):
self.base_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.arm_pub = rospy.Publisher('/arm_1/arm_controller/command',
JointTrajectory,
queue_size=1)
self.gripPub = rospy.Publisher('/arm_1/gripper_controller/command',
JointTrajectory,
queue_size=1)
self.walkerPub = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
self.offset = [
-(5.33 - 5.6), -(4.03 - 3.8), 0 - 0.1, 0 - 0, 0 - 0, 0.4 - 0
]
self.current_base = []
self.base_cmd = Twist()
self.base_target = [5.4, 4.2]
self.eps = 0.01
self.v_max = 0.15
rospy.Subscriber("/gazebo/model_states",
ModelStates,
self.base_callback,
queue_size=1)
self.arm_targets = JointTrajectory()
arm_target1 = JointTrajectoryPoint()
self.gripper_targets = JointTrajectory()
gripper_targets1 = JointTrajectoryPoint()
self.arm_targets.joint_names = [
"arm_joint_1", "arm_joint_2", "arm_joint_3", "arm_joint_4",
"arm_joint_5"
]
self.gripper_targets.joint_names = [
"gripper_finger_joint_l", "gripper_finger_joint_r"
]
arm_target1.positions = [0.03, 0.5, -2.0, 0.9, 0.0]
arm_target1.velocities = [0.5, 0.5, 0.5, 0.5, 0.5]
arm_target1.accelerations = [0.2, 0.2, 0.2, 0.2, 0.2]
arm_target1.effort = [100]
arm_target1.time_from_start = rospy.Duration(0.01)
gripper_targets1.positions = [0.01, 0.01]
gripper_targets1.velocities = [0, 0]
gripper_targets1.accelerations = [0, 0]
gripper_targets1.effort = []
gripper_targets1.time_from_start = rospy.Duration(0.01)
self.arm_targets.points.append(arm_target1)
self.gripper_targets.points.append(gripper_targets1)
def handle_joints_move(req):
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = joint_names
current_joints = get_current_joints()
max_inc = max(
map(lambda (x, y): abs(x - y), zip(req.positions, current_joints)))
if abs(max_inc) < 0.0174:
return QueryJointsMovementResponse(success=1)
j_traj = JointTrajectoryPoint()
#duration = max_inc/req.velocity
duration = 2
j_traj.time_from_start = rospy.Duration(duration)
j_traj.positions = req.positions
j_traj.velocities = [0] * 7
j_traj.accelerations = [0] * 7
j_traj.effort = [0.0] * 7
goal.trajectory.points.append(j_traj)
try:
# Creates the SimpleActionClient, passing the type of the action
# (FollowJointTrajectoryAction) to the constructor.
action_client = actionlib.SimpleActionClient(
action_client_name, FollowJointTrajectoryAction)
# Waits until the action server has started up and started
# listening for goals.
action_client.wait_for_server()
# Send goal
action_client.send_goal(goal)
rospy.sleep(duration)
except rospy.ROSInterruptException as e:
print(str(e))
return QueryJointsMovementResponse(success=1)
def followTrajectory(self, traj):
# Generate ROS trajectory message
# Don't send head pose! plan_runner freaks out
msg = SendJointTrajectoryRequest()
msg.trajectory.header.stamp = rospy.Time.now()
msg.trajectory.header.seq = 0
msg.trajectory.joint_names = self.joint_names[2:]
for sol in traj:
# print "sol:", sol
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration(sol[0])
point.positions = sol[3:]
point.velocities = [0.0]*len(point.positions)
point.accelerations = [0.0]*len(point.positions)
point.effort = [0.0]*len(point.positions)
msg.trajectory.points.append(point)
# Generate ROS head message
# All the head poses are the same -- just use the last one
# print "Moving head to pose:", traj[-1, 1:3]
head_msg = MoveHeadRequest(traj[-1, 1:3])
self.move_head_service(head_msg)
# Call SendTrajectory service
self.traj_service(msg)
def handleJointTrajMsg(data):
#### Initialize Speed Parameter
# axes [l.x l.y l.z a.x a.y a.z]
# scalers = [0.7, 0.7, 0.7, -3.14, -3.14, -3.14]
joint_traj_publisher = rospy.Publisher("robotiq_3f_controller/command", JointTrajectory)
#### Setup JointTraj Publisher
JointTraj = JointTrajectory()
JointTrajPoints = JointTrajectoryPoint()
#### Start Mapping from PoseStamped to JointTraj
JointTraj.header.stamp = rospy.get_rostime()
JointTraj.joint_names = ['finger_1_joint_1','finger_1_joint_2','finger_1_joint_3','finger_2_joint_1','finger_2_joint_2','finger_2_joint_3','finger_middle_joint_1','finger_middle_joint_2','finger_middle_joint_3','palm_finger_1_joint','palm_finger_2_joint']
# data size is 30
JointTrajPoints.positions = data.position
JointTrajPoints.velocities = data.velocity
JointTrajPoints.accelerations = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
JointTrajPoints.effort = [0]
JointTrajPoints.time_from_start = rospy.Duration.from_sec(0.1)
JointTraj.points.append(JointTrajPoints)
# JointTraj.JointTraj.linear.x=data.pose.position.y*clk*scale_factor_pos*-1
# JointTraj.JointTraj.linear.y=data.pose.position.z*clk*scale_factor_pos
# JointTraj.JointTraj.linear.z=data.pose.position.x*clk*scale_factor_pos
# JointTraj.JointTraj.angular.x=data.pose.orientation.x
# JointTraj.JointTraj.angular.y=data.pose.orientation.y
# JointTraj.JointTraj.angular.z=data.pose.orientation.z
#### Publish msg
rate = rospy.Rate(100) # 100hz
rospy.loginfo(data)
joint_traj_publisher.publish(JointTraj)
rate.sleep()
def make_gripper_posture(self, joint_positions):
# 初始化夹爪的关节运动轨迹
t = JointTrajectory()
# 设置夹爪的关节名称
t.joint_names = [
'gripper_left_driver_joint', 'gripper_left_follower_joint',
'gripper_left_spring_link_joint', 'gripper_right_driver_joint',
'gripper_right_follower_joint', 'gripper_right_spring_link_joint'
]
# 初始化关节轨迹点
tp = JointTrajectoryPoint()
# 将输入的关节位置作为一个目标轨迹点
tp.positions = joint_positions
# 设置夹爪的力度
tp.effort = [5.0]
# 设置运动时间
tp.time_from_start = rospy.Duration(1.0)
# 将目标轨迹点加入到运动轨迹中
t.points.append(tp)
# 返回夹爪的关节运动轨迹
return t
def get_trajectory_message(self, action, robot_id=0):
"""
Helper function.
Wraps an action vector of joint angles into a JointTrajectory message.
The velocities, accelerations, and effort do not control the arm motion
"""
# Set up a trajectory message to publish.
action_msg = JointTrajectory()
action_msg.joint_names = self.environment['joint_order']
# Create a point to tell the robot to move to.
target = JointTrajectoryPoint()
action_float = [float(i) for i in action]
# index = 0
# for value in action_float:
# self.kf[index].append(value)
# action_float[index] = self.kf[index].mean()
# index = index + 1
target.positions = action_float
target.velocities = [0.0] * 6
target.effort = [float('nan')] * 6
# These times determine the speed at which the robot moves:
# it tries to reach the specified target position in 'slowness' time.
if (self.slowness_unit == 'sec') or (self.slowness_unit is None):
target.time_from_start.sec = self.slowness
elif (self.slowness_unit == 'nsec'):
target.time_from_start.nanosecs = self.slowness
else:
print("Unrecognized unit. Please use sec or nsec.")
# Package the single point into a trajectory of points with length 1.
action_msg.points = [target]
return action_msg
def get_trajectory_message(self, action,set_const_vel=False, robot_id=0):
"""
Helper function.
Wraps an action vector of joint angles into a JointTrajectory message.
The velocities, accelerations, and effort do not control the arm motion
"""
# Set up a trajectory message to publish.
action_msg = JointTrajectory()
action_msg.joint_names = self.environment['joint_order']
# Create a point to tell the robot to move to.
target = JointTrajectoryPoint()
action_float = [float(i) for i in action]
target.positions = action_float
if set_const_vel:
target.velocities = 0.0005*np.ones(self.scara_chain.getNrOfJoints())
target.effort = 0.005*np.ones(self.scara_chain.getNrOfJoints())
target.time_from_start.secs = 4
else:
# These times determine the speed at which the robot moves:
# it tries to reach the specified target position in 'slowness' time.
if (self.slowness_unit == 'sec') or (self.slowness_unit is None):
target.time_from_start.secs = self.slowness
elif (self.slowness_unit == 'nsec'):
target.time_from_start.nsecs = self.slowness
else:
print("Unrecognized unit. Please use sec or nsec.")
# target.time_from_start.nsecs = self.environment['slowness']
# Package the single point into a trajectory of points with length 1.
action_msg.points = [target]
return action_msg
def thread_joint(self):
rate = rospy.Rate(10) # 10hz
max_vel = 30
while not rospy.is_shutdown():
traj = JointTrajectory()
traj.joint_names = ['finger_joint1', 'finger_joint2']
point = JointTrajectoryPoint()
pos = self.joint_pose[0]
cmd = self.joint_cmd[0]
point.positions.append(cmd)
point.velocities.append(-(cmd - pos) * max_vel)
#point.accelerations.append(5)
point.time_from_start.secs = 1
point.time_from_start.nsecs = 0
pos = self.joint_pose[1]
cmd = self.joint_cmd[1]
point.positions.append(cmd)
point.velocities.append(-(cmd - pos) * max_vel)
#point.accelerations.append(5)
point.time_from_start.secs = 1
point.time_from_start.nsecs = 0
point.effort = [100, 100]
traj.points.append(point)
self.joint_pub.publish(traj)
rate.sleep()
def move_arm(self, points_list):
# # Unpause the physics
# rospy.wait_for_service('/gazebo/unpause_physics')
# unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
# unpause_gazebo()
self.client.wait_for_server()
goal = FollowJointTrajectoryGoal()
# We need to fill the goal message with its components
#
# check msg structure with: rosmsg info FollowJointTrajectoryGoal
# It is composed of 4 sub-messages:
# "trajectory" of type trajectory_msgs/JointTrajectory
# "path_tolerance" of type control_msgs/JointTolerance
# "goal_tolerance" of type control_msgs/JointTolerance
# "goal_time_tolerance" of type duration
trajectory_msg = JointTrajectory()
# check msg structure with: rosmsg info JointTrajectory
# It is composed of 3 sub-messages:
# "header" of type std_msgs/Header
# "joint_names" of type string
# "points" of type trajectory_msgs/JointTrajectoryPoint
trajectory_msg.joint_names = [
"j2n6s300_joint_1", "j2n6s300_joint_2", "j2n6s300_joint_3",
"j2n6s300_joint_4", "j2n6s300_joint_5", "j2n6s300_joint_6"
]
points_msg = JointTrajectoryPoint()
# check msg structure with: rosmsg info JointTrajectoryPoint
# It is composed of 5 sub-messages:
# "positions" of type float64
# "velocities" of type float64
# "accelerations" of type float64
# "efforts" of type float64
# "time_from_start" of type duration
points_msg.positions = [0, 0, 0, 0, 0, 0]
points_msg.velocities = [0, 0, 0, 0, 0, 0]
points_msg.accelerations = [0, 0, 0, 0, 0, 0]
points_msg.effort = [0, 1, 0, 0, 0, 0]
points_msg.time_from_start = rospy.Duration(0.01)
# fill in points message of the trajectory message
trajectory_msg.points = [points_msg]
# fill in trajectory message of the goal
goal.trajectory = trajectory_msg
print(trajectory_msg)
# self.client.send_goal_and_wait(goal)
self.client.send_goal(goal)
self.client.wait_for_result()
rospy.sleep(2) # wait for 2s
def add_point(self, positions, time):
point = JointTrajectoryPoint()
point.positions = copy(positions)
point.velocities = copy(np.zeros(len(positions)))
point.accelerations = copy(np.zeros(len(positions)))
point.effort = copy(np.zeros(len(positions)))
point.time_from_start = rospy.Duration(time)
self._goal.trajectory.points.append(point)
def make_gripper_posture(self, pose):
t = JointTrajectory()
t.joint_names = gripper_joint_names
tp = JointTrajectoryPoint()
tp.positions = [pose / 2.0 for j in t.joint_names]
tp.effort = gripper_effort
t.points.append(tp)
return t
def make_gripper_posture(self, pose):
t = JointTrajectory()
t.joint_names = GRIPPER_JOINT_NAMES
tp = JointTrajectoryPoint()
tp.positions = [pose for j in t.joint_names]
tp.effort = GRIPPER_EFFORT
t.points.append(tp)
return t
def make_gripper_posture(self, pose):
t = JointTrajectory()
t.joint_names = GRIPPER_JOINT_NAMES
tp = JointTrajectoryPoint()
tp.positions = [pose for j in t.joint_names]
tp.effort = GRIPPER_EFFORT
t.points.append(tp)
return t
def talker():
if 1 == 0:
pub = rospy.Publisher("/ihmc_ros/atlas/control/arm_joint_trajectory2", JointTrajectory, queue_size=10)
jn = ["l_arm_shz", "l_arm_shx", "l_arm_ely", "l_arm_elx", "l_arm_wry", "l_arm_wrx", "l_arm_wry2"]
jn_r = ["r_arm_shz", "r_arm_shx", "r_arm_ely", "r_arm_elx", "r_arm_wry", "r_arm_wrx", "r_arm_wry2"]
else:
pub = rospy.Publisher("/ihmc_ros/valkyrie/control/arm_joint_trajectory2", JointTrajectory, queue_size=10)
jn = [
"LeftShoulderPitch",
"LeftShoulderRoll",
"LeftShoulderYaw",
"LeftElbowPitch",
"LeftForearmYaw",
"LeftWristRoll",
"LeftWristPitch",
]
jn_r = [
"RightShoulderPitch",
"RightShoulderRoll",
"RightShoulderYaw",
"RightElbowPitch",
"RightForearmYaw",
"RightWristRoll",
"RightWristPitch",
]
# this doesnt work:
# jn = ["l_arm_shz","l_arm_shx","l_arm_ely","l_arm_elx","l_arm_wry","l_arm_wrx","l_arm_wry2", "r_arm_shz","l_arm_shx","r_arm_ely","r_arm_elx","r_arm_wry","r_arm_wrx","r_arm_wry2"]
# value = 0
rospy.init_node("send_arm_test", anonymous=True)
rate = rospy.Rate(1) # 10hz
while not rospy.is_shutdown():
msg = JointTrajectory()
value = 0.1
value_r = 0.1
msg.joint_names = jn
pt = JointTrajectoryPoint()
pt.positions = [value] * len(jn)
pt.velocities = [0] * len(jn)
pt.accelerations = [0] * len(jn)
pt.effort = [0] * len(jn)
pt.time_from_start = rospy.Duration.from_sec(3)
msg.points.append(pt)
print msg.joint_names, rospy.get_time()
pub.publish(msg)
# TODO: add a sleep here between left and right
msg.joint_names = jn_r
msg.points[0].positions = [value_r] * len(jn)
print msg.joint_names, rospy.get_time()
pub.publish(msg)
rate.sleep()
def make_gripper_posture(self, pose):
t = JointTrajectory()
# t.joint_names = ['finger_joint','left_inner_finger_joint','left_inner_knuckle_joint','left_inner_knuckle_dummy_joint','right_inner_knuckle_joint','right_inner_knuckle_dummy_joint','right_outer_knuckle_joint','right_inner_finger_joint']
t.joint_names = ['finger_joint']
tp = JointTrajectoryPoint()
tp.positions = [pose for j in t.joint_names]
tp.effort = [0.0]
t.points.append(tp)
return t
def make_joint_trajectory(names, points):
jt = JointTrajectory()
jt.joint_names = names
pt = JointTrajectoryPoint()
pt.positions = points
pt.effort = [0]*len(points)
pt.velocities = [0]*len(points)
pt.accelerations = [0]*len(points)
jt.points = [pt]
return jt
def publish_eye_pos(eye_publisher, val):
traj = JointTrajectory()
traj.joint_names = ["eyelids_joint"]
p = JointTrajectoryPoint()
p.positions = [val]
p.velocities = []
p.effort = []
p.time_from_start = rospy.Time(1)
traj.points = [p]
eye_publisher.publish(traj)
def jointTrajectoryPointFromJointState(jointState, timeFromStart):
trajPoint = JointTrajectoryPoint()
trajPoint.positions = jointState.position
numJoints = len(jointState.position)
trajPoint.velocities = [0] * numJoints
trajPoint.accelerations = [0] * numJoints
trajPoint.effort = [0] * numJoints
trajPoint.time_from_start = timeFromStart
return trajPoint
def create_joint_trajectory(self, hand_joints, position, effort):
jt = JointTrajectory()
jt.joint_names = hand_joints
#jt.joint_names.append('r_gripper_joint') ##
jtp = JointTrajectoryPoint()
jtp.positions = position
#jtp.positions.append(0.08) ##
jtp.effort = effort
#jtp.effort.append(100.0) ##
jt.points.append(jtp)
return jt
def move_jtp(self, pos):
jtp_msg = JointTrajectory()
jtp_msg.joint_names = self.joint_name_lst
point = JointTrajectoryPoint()
point.positions = pos
point.velocities = self.jtp_zeros
point.accelerations = self.jtp_zeros
point.effort = self.jtp_zeros
point.time_from_start = rospy.Duration(1.0 / 60.0)
jtp_msg.points.append(point)
self.jtp.publish(jtp_msg)
def publish_head_pos(head_publisher, pan, tilt):
traj = JointTrajectory()
traj.joint_names = ["head_1_joint", "head_2_joint"]
p = JointTrajectoryPoint()
p.positions = [pan, tilt]
p.velocities = []
p.effort = []
p.time_from_start = rospy.Time(1)
traj.points = [p]
head_publisher.publish(traj)
def pan_and_tilt(self,
pan,
tilt,
duration=0.5,
effort_pan=1.0,
effort_tilt=1.0,
feedback_cb=None,
done_cb=None):
"""
Moves the robot's head to the point specified in the duration
specified
:param pan: The pan - expected to be between HeadClient.PAN_LEFT
and HeadClient.PAN_RIGHT
:param tilt: The tilt - expected to be between HeadClient.TILT_UP
and HeadClient.TILT_DOWNs
:param duration: The amount of time to take to get the head to
the specified location.
:param feedback_cb: Same as send_trajectory's feedback_cb
:param done_cb: Same as send_trajectory's done_cb
"""
# Build a JointTrajectoryPoint that expresses the target configuration
if pan > self.PAN_LEFT:
pan = self.PAN_LEFT
elif pan < self.PAN_RIGHT:
pan = self.PAN_RIGHT
if tilt < self.TILT_UP:
tilt = self.TILT_UP
elif tilt > self.TILT_DOWN:
tilt = self.TILT_DOWN
# Build a JointTrajectoryPoint that expresses the target configuration
point = JointTrajectoryPoint()
point.positions = [pan, tilt]
point.effort = [effort_pan, effort_tilt]
duration_in_nsec = duration * 1e9
point.time_from_start.nsecs = duration_in_nsec
# Put that point into the right container type, and target the
# correct joint.
trajectory = JointTrajectory()
trajectory.joint_names = [self.JOINT_PAN, self.JOINT_TILT]
trajectory.points = [point]
return self.send_trajectory(traj=trajectory,
feedback_cb=feedback_cb,
done_cb=done_cb)
"""
def make_grab_posture(self, joint_positions):
t = JointTrajectory()
t.header.stamp = rospy.get_rostime()
t.joint_names = self.hand_group.get_joints()
tp= JointTrajectoryPoint()
tp.positions = joint_positions
tp.effort = [1.0]
tp.time_from_start = rospy.Duration(0.0)
t.points.append(tp)
return t
def send_joints_positions(self,
joints_positions_array,
seconds_duration=0.05):
my_goal = self.get_goal()
my_goal.trajectory.header.stamp = rospy.Time.now()
joint_traj_point = JointTrajectoryPoint()
# We clamp the values to max and min to avoid asking configurations that IriWam cant reach.
joint_traj_point.positions = numpy.clip(joints_positions_array,
self.min_values,
self.max_values).tolist()
joint_traj_point.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
joint_traj_point.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
joint_traj_point.effort = []
joint_traj_point.time_from_start = rospy.Duration.from_sec(
seconds_duration)
my_goal.trajectory.points = []
my_goal.trajectory.points.append(joint_traj_point)
# sends the goal to the action server, specifying which feedback function
# to call when feedback received
self.client.send_goal(my_goal, feedback_cb=self.feedback_callback)
# Uncomment these lines to test goal preemption:
# self.client.cancel_goal() # would cancel the goal 3 seconds after starting
state_result = self.client.get_state()
rate = rospy.Rate(10)
rospy.loginfo("state_result: " + str(state_result))
while state_result < self.DONE:
rospy.loginfo(
"Doing Stuff while waiting for the Server to give a result...."
)
rate.sleep()
state_result = self.client.get_state()
rospy.loginfo("state_result: " + str(state_result))
rospy.loginfo("[Result] State: " + str(state_result))
if state_result == self.ERROR:
rospy.logerr("Something went wrong in the Server Side")
if state_result == self.WARN:
rospy.logwarn("There is a warning in the Server Side")
def make_gripper_posture(joint_positions):
#inicilizo el joint trayectory para los joint de la pinza
t = JointTrajectory()
#Defino los nombres de los joint de la pinza, en mi caso no se si solo se pone el 1 o los 2
t.joint_names = GRIPPER_JOINT_NAMES
#Asigno la una trayectoria de punto que represente el objetivo
tp = JointTrajectoryPoint()
#Asigno la trayectoria a los joints
tp.positions = joint_positions
#FIjo el gripper effort
tp.effort = GRIPPER_EFFORT
tp.time_from_start = rospy.Duration(1.0)
#adjunto los puntos a la trayectoria de puntos
t.points.append(tp)
return t
def create_trajectory_point(position, seconds):
"""
Create a JointTrajectoryPoint
:param position: Joint positions
:param seconds: Time from start in seconds
:returns: JointTrajectoryPoint
"""
point = JointTrajectoryPoint()
point.positions.extend(position)
point.velocities = [0.001, 0.001, 0.001]
point.accelerations = [0.0001, 0.0001, 0.0001]
point.effort = [10000, 10000, 10000]
point.time_from_start = rospy.Duration(seconds)
return point
def move_arm(self, angles): #input: angles
goal = JointTrajectoryGoal()
char = self.arm_name[0] #either 'r' or 'l'
goal.trajectory.joint_names = [
char + '_shoulder_pan_joint', char + '_shoulder_lift_joint',
char + '_upper_arm_roll_joint', char + '_elbow_flex_joint',
char + '_forearm_roll_joint', char + '_wrist_flex_joint',
char + '_wrist_roll_joint'
]
point = JointTrajectoryPoint()
point.effort = angles
point.time_from_start = rospy.Duration(3)
goal.trajectory.points.append(point)
self.jta.send_goal_and_wait(goal)
def get_trajectory_message(self, action, robot_id=0):
"""
Helper function.
Wraps an action vector of joint angles into a JointTrajectory message.
The velocities, accelerations, and effort do not control the arm motion
"""
dt = time.time() - self.last_time
# Set up a trajectory message to publish.
action_msg = JointTrajectory()
action_msg.joint_names = self.environment['joint_order']
# Create a point to tell the robot to move to.
target = JointTrajectoryPoint()
if (self._observation_msg.actual.positions[0] > action[0]):
self.goal_vel1 = -self.goal_vel_value
else:
self.goal_vel1 = self.goal_vel_value
self.goal_cmd1 += dt * self.goal_vel1
if (self._observation_msg.actual.positions[1] > action[1]):
self.goal_vel2 = -self.goal_vel_value
else:
self.goal_vel2 = self.goal_vel_value
self.goal_cmd2 += dt * self.goal_vel2
if (self._observation_msg.actual.positions[2] > action[2]):
self.goal_vel3 = -self.goal_vel_value
else:
self.goal_vel3 = self.goal_vel_value
self.goal_cmd3 += dt * self.goal_vel3
target.positions = [self.goal_cmd1, self.goal_cmd2, self.goal_cmd3]
target.velocities = [self.goal_vel_value] * 3
target.effort = [float('nan')] * 3
# These times determine the speed at which the robot moves:
# it tries to reach the specified target position in 'slowness' time.
if (self.slowness_unit == 'sec') or (self.slowness_unit is None):
target.time_from_start.secs = self.slowness
elif (self.slowness_unit == 'nsec'):
target.time_from_start.nsecs = self.slowness
else:
print("Unrecognized unit. Please use sec or nsec.")
# target.time_from_start.nsecs = self.environment['slowness']
# Package the single point into a trajectory of points with length 1.
action_msg.points = [target]
self.last_time = time.time()
return action_msg
def callback(data):
jp = JointTrajectoryPoint()
jp.positions = [0, 0] # in ordine definito dal names
jp.velocities = []
jp.accelerations = []
jp.effort = []
print jp
joints = JointTrajectory()
joints.header.stamp = rospy.Time.now()
joints.header.frame_id = '/base_link'
joints.joint_names = ['pan', 'tilt']
joints.points.append(jp)
head_pub.publish(joints)
def start(self):
self.start = rospy.Time.now()
rate = rospy.Rate(10)
s = 0
while not (rospy.is_shutdown() or s >= len(self.x_discretized)):
# Build JointTrajectory message
header = Header()
header.seq = s
header.stamp = rospy.get_rostime()
header.frame_id = 'inertial frame'
joint_trajectory_msg = JointTrajectory()
joint_trajectory_msg.header = header
joint_trajectory_msg.joint_names = ['drone']
# Build JointTrajectoryPoint
for i in range(min(self.WINDOW_FRAME,
len(self.x_discretized) - s)):
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [
self.x_discretized[s + i], self.y_discretized[s + i],
self.z_discretized[s + i], self.ya_discretized[s + i]
]
joint_trajectory_point.velocities = [
self.vx_discretized[s + i], self.vy_discretized[s + i],
self.vz_discretized[s + i]
] if i != (self.WINDOW_FRAME - 1) else [.0, .0, .0]
joint_trajectory_point.accelerations = [
self.ax_discretized[s + i], self.ay_discretized[s + i],
self.az_discretized[s + i]
] if i != (self.WINDOW_FRAME - 1) else [.0, .0, .0]
joint_trajectory_point.effort = [None]
joint_trajectory_point.time_from_start = self.ti_discretized[s
+
i]
joint_trajectory_msg.points.append(joint_trajectory_point)
s = s + 1
# trajectory = "new trajectory %s" % rospy.get_time()
rospy.loginfo('##########################################')
rospy.loginfo(joint_trajectory_msg)
self.pub.publish(joint_trajectory_msg)
rate.sleep()
def execute_cb(self, goal):
rospy.loginfo('Receving controller trajectories...')
jt = JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.joint_names = goal.trajectory.joint_names
jt.points = []
for point in goal.trajectory.points:
jtp = JointTrajectoryPoint()
jtp.positions = point.positions
jtp.velocities = point.velocities
jtp.accelerations = point.accelerations
jtp.effort = point.effort
jtp.time_from_start = point.time_from_start
jt.points.append(jtp)
self.dx_trajectory_pub.publish(jt)
result_ = FollowJointTrajectoryResult()
self._as.set_succeeded(result_)
rospy.loginfo('Joint trajectories has been successfuly transmitted to dynamixel branch.')
def _get_trajectory_msg(self):
"""
Constructs a trajectory message that has only one set point to be
standing still in the idle position after the specified duration.
:return:
"""
msg = JointTrajectory()
msg.joint_names = sorted(self._position.keys())
point = JointTrajectoryPoint()
point.time_from_start = self._duration.to_msg()
point.positions = [self._position[name] for name in msg.joint_names]
point.velocities = [0.0] * len(msg.joint_names)
point.accelerations = [0.0] * len(msg.joint_names)
point.effort = [0.0] * len(msg.joint_names)
msg.points = [point]
return msg
def make_gripper_posture(self, joint_positions):
# Initialize the joint trajectory for the gripper joints
t = JointTrajectory()
# Set the joint names to the gripper joint names
t.joint_names = GRIPPER_JOINT_NAMES
# Initialize a joint trajectory point to represent the goal
tp = JointTrajectoryPoint()
# Assign the trajectory joint positions to the input positions
tp.positions = joint_positions
# Set the gripper effort
tp.effort = GRIPPER_EFFORT
tp.time_from_start = rospy.Duration(1.0)
# Append the goal point to the trajectory points
t.points.append(tp)
return t
def sendWaypointTrajectory(self, waypoints, durations = 0., vels = 0., accs = 0., effs = 0.):
if not self.ang_cmd_wait(waypoints[0]):
print 'Cannot go to the first point in the trajectory'
return None
# else:
# print 'Went to first'
if not self.traj_connection:
print 'Action server connection was not established'
return None
joint_traj = JointTrajectory()
joint_traj.joint_names = self.arm_joint_names;
if not durations == 0:
if not len(durations) == waypoints:
raise Exception('The number of duration points is not equal to the number of provided waypoints')
if not vels == 0:
if not len(vels) == waypoints:
raise Exception('The number velocity points is not equal to the number of provided waypoints')
if not accs == 0:
if not len(accs) == waypoints:
raise Exception('The number acceleration points is not equal to the number of provided waypoints')
if not effs == 0:
if not len(effs) == waypoints:
raise Exception('The number effort points is not equal to the number of provided waypoints')
if not effs == 0:
if not (vels == 0 and accs == 0):
raise Exception('Cannot specify efforts with velocities and accelerations at the same time')
if (not accs == 0) and vels == 0:
raise Exception('Cannot specify accelerations without velocities')
total_time_from_start = 0.5;
for t in range(0, len(waypoints)):
point = JointTrajectoryPoint()
waypoint = waypoints[t]
if not len(waypoint) == len(joint_traj.joint_names):
raise Exception('The number of provided joint positions is not equal to the number of available joints for index: ' + str(t))
point.positions = waypoint
if not vels == 0.:
velocity = vels[t]
if not len(velocity) == len(joint_traj.joint_names):
raise Exception('The number of provided joint velocities is not equal to the number of available joints for index: ' + str(t))
point.velocities = velocity
if not accs == 0.:
acceleration = accs[t]
if not len(acceleration) == len(joint_traj.joint_names):
raise Exception('The number of provided joint accelerations is not equal to the number of available joints for index: ' + str(t))
point.accelerations = accelerations
if not effs == 0.:
effort = effs[t]
if not len(effort) == len(joint_traj.joint_names):
raise Exception('The number of provided joint efforts is not equal to the number of available joints for index: ' + str(t))
point.effort = effort
if not durations == 0.:
point.duration = duration
# Deal with increasing time for each trajectory point
point.time_from_start = rospy.Duration(total_time_from_start)
total_time_from_start = total_time_from_start + 1.0
# Set the points
joint_traj.points.append(point)
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = joint_traj
self.smooth_joint_trajectory_client.send_goal(traj_goal)
self.smooth_joint_trajectory_client.wait_for_result()
return self.smooth_joint_trajectory_client.get_result()
