def scale_trajectory_speed(traj, scale):
# Create a new trajectory object
new_traj = RobotTrajectory()
# Initialize the new trajectory to be the same as the planned trajectory
new_traj.joint_trajectory = traj.joint_trajectory
# Get the number of joints involved
n_joints = len(traj.joint_trajectory.joint_names)
# Get the number of points on the trajectory
n_points = len(traj.joint_trajectory.points)
# Store the trajectory points
points = list(traj.joint_trajectory.points)
# Cycle through all points and scale the time from start, speed and acceleration
for i in range(n_points):
point = JointTrajectoryPoint()
point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
point.velocities = list(traj.joint_trajectory.points[i].velocities)
point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
point.positions = traj.joint_trajectory.points[i].positions
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * scale
point.accelerations[j] = point.accelerations[j] * scale * scale
points[i] = point
# Assign the modified points to the new trajectory
new_traj.joint_trajectory.points = points
# Return the new trajectory
return new_traj
def main():
pub = rospy.Publisher('sevenbot/joint_traj_controller/trajectory_command',JointTrajectory)
rospy.init_node('traj_test')
p1 = JointTrajectoryPoint()
p1.positions = [0,0,0,0,0,0,0]
p1.velocities = [0,0,0,0,0,0,0]
p1.accelerations = [0,0,0,0,0,0,0]
p2 = JointTrajectoryPoint()
p2.positions = [0,0,1.0,0,-1.0,0,0]
p2.velocities = [0,0,0,0,0,0,0]
p2.accelerations = [0,0,0,0,0,0,0]
p2.time_from_start = rospy.Time(4.0)
p3 = JointTrajectoryPoint()
p3.positions = [0,0,-1.0,0,1.0,0,0]
p3.velocities = [0,0,0,0,0,0,0]
p3.accelerations = [0,0,0,0,0,0,0]
p3.time_from_start = rospy.Time(20.0)
traj = JointTrajectory()
traj.joint_names = ['j1','j2','j3','j4','j5','j6','j7']
traj.points = [p1,p2,p3]
rospy.loginfo(traj)
r = rospy.Rate(1) # 10hz
pub.publish(traj)
r.sleep()
pub.publish(traj)
def add_point(self, positions, velocities, accelerations, time):
point = JointTrajectoryPoint()
point.positions = copy(positions)
point.velocities = copy(velocities)
point.accelerations = copy(accelerations)
point.time_from_start = rospy.Duration(time)
self._goal.trajectory.points.append(point)
def make_trajectory_msg(plan=[], seq=0, secs=0, nsecs=0, dt=2, frame_id='/base_link'):
'''
This function will convert the plan to a joint trajectory compatible with the
/arm_N/arm_controller/command topic
'''
theplan = plan
# create joint trajectory message
jt = JointTrajectory()
# fill the header
jt.header.seq = seq
jt.header.stamp.secs = 0 #secs
jt.header.stamp.nsecs = 0 #nsecs
jt.header.frame_id = frame_id
# specify the joint names
jt.joint_names = theplan.joint_trajectory.joint_names
# fill the trajectory points and computer constraint times
njtp = len(theplan.joint_trajectory.points)
for ii in range(0, njtp):
jtp = JointTrajectoryPoint()
jtp = copy.deepcopy(theplan.joint_trajectory.points[ii])
jtp.velocities = [0.0, 0.0, 0.0, 0.0, 0.0]
jtp.accelerations =[0.0, 0.0, 0.0, 0.0, 0.0]
jtp.time_from_start = rospy.Duration.from_sec(secs + dt*(ii+1))
jt.points.append(jtp)
return jt
def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
if self._mode == "velocity":
velocities = [0.0] * len(joint_names)
cmd = dict(zip(joint_names, velocities))
while not self._server.is_new_goal_available() and self._alive:
self._limb.set_joint_velocities(cmd)
if self._cuff_state:
self._limb.exit_control_mode()
break
rospy.sleep(1.0 / self._control_rate)
elif self._mode == "position" or self._mode == "position_w_id":
raw_pos_mode = self._mode == "position_w_id"
if raw_pos_mode:
pnt = JointTrajectoryPoint()
pnt.positions = self._get_current_position(joint_names)
if dimensions_dict["velocities"]:
pnt.velocities = [0.0] * len(joint_names)
if dimensions_dict["accelerations"]:
pnt.accelerations = [0.0] * len(joint_names)
while not self._server.is_new_goal_available() and self._alive:
self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode)
# zero inverse dynamics feedforward command
if self._mode == "position_w_id":
pnt.time_from_start = rospy.Duration(rospy.get_time() - start_time)
ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt)
self._pub_ff_cmd.publish(ff_pnt)
if self._cuff_state:
self._limb.exit_control_mode()
break
rospy.sleep(1.0 / self._control_rate)
def solution2hand_joint_path(solution, robot, total_time):
trajectory = JointTrajectory()
trajectory.joint_names = ["left_hand_synergy_joint"]
n_waypoints = len(solution['joint_trajectory'])
time_sec = 0.0001
time_incr = float(total_time) / n_waypoints
for i in range(n_waypoints):
#extrapolation = 1.1
joint_cmd = np.max(solution['joint_trajectory'][i])
if i >= n_waypoints - 2:
joint_cmd = np.minimum(joint_cmd * 1.5, 1.0)
else:
joint_cmd = joint_cmd
rospy.loginfo("Joint command %d = %f" % (i, joint_cmd))
joint_positions = [joint_cmd]
point = JointTrajectoryPoint()
point.positions = joint_positions
point.velocities = [0.0] * len(joint_positions)
point.accelerations = [0.0] * len(joint_positions)
point.time_from_start = rospy.Duration(time_sec)
trajectory.points.append(point)
time_sec += time_incr
return trajectory
def scale_trajectory_speed(traj, scale):
new_traj = RobotTrajectory()
new_traj.joint_trajectory = traj.joint_trajectory
n_joints = len(traj.joint_trajectory.joint_names)
n_points = len(traj.joint_trajectory.points)
points = list(traj.joint_trajectory.points)
for i in range(n_points):
point = JointTrajectoryPoint()
point.positions = traj.joint_trajectory.points[i].positions
point.time_from_start = traj.joint_trajectory.points[
i].time_from_start / scale
point.velocities = list(traj.joint_trajectory.points[i].velocities)
point.accelerations = list(
traj.joint_trajectory.points[i].accelerations)
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * scale
point.accelerations[j] = point.accelerations[j] * scale * scale
points[i] = point
new_traj.joint_trajectory.points = points
return new_traj
def runTrajectory(req):
global Traj_server;
print "---------------------------------"
print req.task
print " "
print req.bin_num
print " "
print req.using_torso
print "---------------------------------"
if req.using_torso == "y":
file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/Torso/bin"+req.bin_num);
else:
file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/bin"+req.bin_num);
if req.task == "Forward":
file_name = file_root+"/forward";
elif req.task == "Drop":
file_name = file_root+"/drop";
else :
return taskResponse(False);
f = open(file_name,"r");
plan_file = RobotTrajectory();
buf = f.read();
plan_file.deserialize(buf);
plan = copy.deepcopy(plan_file);
arm_right_group = moveit_commander.MoveGroupCommander("arm_right");
arm_left_group = moveit_commander.MoveGroupCommander("arm_left");
arm_left_group.set_start_state_to_current_state();
StartPnt = JointTrajectoryPoint();
StartPnt.positions = arm_left_group.get_current_joint_values();
StartPnt.velocities = [0]*len(StartPnt.positions);
StartPnt. accelerations = [0]*len(StartPnt.positions);
#print StartPnt;
plan.joint_trajectory.points[0] = StartPnt;
#print plan;
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory)
display_trajectory = moveit_msgs.msg.DisplayTrajectory();
robot = moveit_commander.RobotCommander();
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory);
print "============ Waiting while", file_name, " is visualized (again)..."
arm_left_group.execute(plan);
print "Trajectory ", file_name, " finished!"
f.close();
return taskResponse(True);
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'
]
for i in range(len(traj)):
point = JointTrajectoryPoint()
p = [0] * 6
v = [0] * 6
last_v = [0] * 6
a = [0] * 6
if i != len(traj) - 1 and i != 0:
for j in range(6):
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 __extend_trajectory_helper(self, plan, start_pose, travel_distance, step=0.005, time_step=0.1):
traj = plan.joint_trajectory.points
cur_pose_z = start_pose.position.z
start_time = plan.joint_trajectory.points[-1].time_from_start
for i in range(0, int(abs(travel_distance) / step)):
point = JointTrajectoryPoint()
print i
joint_goal = self.__inverse_kinematics([start_pose.position.x, start_pose.position.y, cur_pose_z + (np.sign(travel_distance) * step)],
[start_pose.orientation.w, start_pose.orientation.x, start_pose.orientation.y, start_pose.orientation.z], NIC_SEED_STATE)
if joint_goal == None:
print "No IK on extend trajectory found"
return None
joint_list = []
for idx, jnt in enumerate(joint_goal.tolist()):
joint_list.append(jnt)
point.positions = joint_list
print joint_list
point.velocities = [0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.0]
point.accelerations = [0,0,0,0,0,0,0]
time = rospy.Duration.from_sec(start_time.to_sec() + time_step)
start_time += rospy.Duration.from_sec(time_step)
point.time_from_start = time
traj.append(point)
cur_pose_z += np.sign(travel_distance) * step
return plan
def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
if self._mode == 'velocity':
velocities = [0.0] * len(joint_names)
cmd = dict(zip(joint_names, velocities))
while (not self._server.is_new_goal_available() and self._alive and self.robot_is_enabled()):
self._limb.set_joint_velocities(cmd)
if self._cuff_state:
self._limb.exit_control_mode()
break
rospy.sleep(1.0 / self._control_rate)
elif self._mode == 'position' or self._mode == 'position_w_id':
raw_pos_mode = (self._mode == 'position_w_id')
if raw_pos_mode:
pnt = JointTrajectoryPoint()
pnt.positions = self._get_current_position(joint_names)
if dimensions_dict['velocities']:
pnt.velocities = [0.0] * len(joint_names)
if dimensions_dict['accelerations']:
pnt.accelerations = [0.0] * len(joint_names)
while (not self._server.is_new_goal_available() and self._alive and self.robot_is_enabled()):
self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode)
# zero inverse dynamics feedforward command
if self._mode == 'position_w_id':
pnt.time_from_start = rospy.Duration(rospy.get_time() - start_time)
ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt)
self._pub_ff_cmd.publish(ff_pnt)
if self._cuff_state:
self._limb.exit_control_mode()
break
rospy.sleep(1.0 / self._control_rate)
def traj_speed_up(traj, spd=3.0):
new_traj = RobotTrajectory()
new_traj = traj
n_joints = len(traj.joint_trajectory.joint_names)
n_points = len(traj.joint_trajectory.points)
points = list(traj.joint_trajectory.points)
for i in range(n_points):
point = JointTrajectoryPoint()
point.time_from_start = traj.joint_trajectory.points[i].time_from_start / spd
point.velocities = list(traj.joint_trajectory.points[i].velocities)
point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
point.positions = traj.joint_trajectory.points[i].positions
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * spd
point.accelerations[j] = point.accelerations[j] * spd * spd
points[i] = point
new_traj.joint_trajectory.points = points
return new_traj
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 scale_trajectory_speed(traj, scale):
# Create a new trajectory object
new_traj = RobotTrajectory()
# Initialize the new trajectory to be the same as the planned trajectory
new_traj.joint_trajectory = traj.joint_trajectory
# Get the number of joints involved
n_joints = len(traj.joint_trajectory.joint_names)
# Get the number of points on the trajectory
n_points = len(traj.joint_trajectory.points)
# Store the trajectory points
points = list(traj.joint_trajectory.points)
# Cycle through all points and scale the time from start, speed and acceleration
for i in range(n_points):
point = JointTrajectoryPoint()
point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
point.velocities = list(traj.joint_trajectory.points[i].velocities)
point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
point.positions = traj.joint_trajectory.points[i].positions
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * scale
point.accelerations[j] = point.accelerations[j] * scale * scale
points[i] = point
# Assign the modified points to the new trajectory
new_traj.joint_trajectory.points = points
# Return the new trajecotry
return new_traj
def make_trajectory_msg(plan=[], seq=0, secs=0, nsecs=0, dt=2, frame_id="/base_link"):
"""
This function will convert the plan to a joint trajectory compatible with the
/arm_N/arm_controller/command topic
"""
theplan = plan
# create joint trajectory message
jt = JointTrajectory()
# fill the header
jt.header.seq = seq
jt.header.stamp.secs = 0 # secs
jt.header.stamp.nsecs = 0 # nsecs
jt.header.frame_id = frame_id
# specify the joint names
jt.joint_names = theplan.joint_trajectory.joint_names
# fill the trajectory points and computer constraint times
njtp = len(theplan.joint_trajectory.points)
for ii in range(0, njtp):
jtp = JointTrajectoryPoint()
jtp = copy.deepcopy(theplan.joint_trajectory.points[ii])
jtp.velocities = [0.0, 0.0, 0.0, 0.0, 0.0]
jtp.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0]
jtp.time_from_start = rospy.Duration.from_sec(secs + dt * (ii + 1))
jt.points.append(jtp)
return jt
def ProcessTrajectory(self,
trajectory,
pres_joint_pos,
string_trajectories=False):
joint_names = trajectory.joint_names
num_joints = len(joint_names)
trajectory_points = trajectory.points
pnt_times = [0.0] * len(trajectory_points)
dimensions_dict = self._determine_dimensions(trajectory_points)
# Create a new discretized joint trajectory
num_points = len(trajectory_points)
if num_points == 0:
rospy.logerr("%s: Empty Trajectory" % (action_name, ))
return False, []
if num_points == 1:
# Add current position as trajectory point
first_trajectory_point = JointTrajectoryPoint()
first_trajectory_point.positions = [
pres_joint_pos[jnt] for jnt in joint_names
]
# To preserve desired velocities and accelerations, copy them to the first
# trajectory point if the trajectory is only 1 point.
if dimensions_dict['velocities']:
first_trajectory_point.velocities = deepcopy(
trajectory_points[0].velocities)
if dimensions_dict['accelerations']:
first_trajectory_point.accelerations = deepcopy(
trajectory_points[0].accelerations)
first_trajectory_point.time_from_start = rospy.Duration(0)
trajectory_points.insert(0, first_trajectory_point)
num_points = len(trajectory_points)
# Force Velocites/Accelerations to zero at the final timestep
# if they exist in the trajectory
# Remove this behavior if you are stringing together trajectories,
# and want continuous, non-zero velocities/accelerations between
# trajectories
if not string_trajectories:
if dimensions_dict['velocities']:
trajectory_points[-1].velocities = [0.0] * len(joint_names)
if dimensions_dict['accelerations']:
trajectory_points[-1].accelerations = [0.0] * len(joint_names)
# Compute Full Bezier Curve Coefficients for all 7 joints
pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
try:
b_matrix = self._compute_bezier_coeff(joint_names,
trajectory_points,
dimensions_dict)
except Exception as ex:
rospy.logerr(("{0}: Failed to compute a Bezier trajectory for {1}"
" arm with error \"{2}: {3}\"").format(
self._action_name, self._name,
type(ex).__name__, ex))
return False, []
return True, [
dimensions_dict, b_matrix, trajectory_points, pnt_times, num_points
]
def cartesian_path2joint_path(waypoints, robot):
trajectory = JointTrajectory()
trajectory.joint_names = robot.joint_names('left_arm')
time_sec = 0.5
time_incr = 0.5
for i in range(len(waypoints)):
wpt = waypoints[i]
joint_positions = robot.inverse_kinematics(
[wpt.position.x, wpt.position.y, wpt.position.z], [
wpt.orientation.x, wpt.orientation.y, wpt.orientation.z,
wpt.orientation.w
])
if joint_positions is not None:
point = JointTrajectoryPoint()
point.positions = joint_positions
point.velocities = [0.0] * len(joint_positions)
point.accelerations = [0.0] * len(joint_positions)
point.time_from_start = rospy.Duration(time_sec)
trajectory.points.append(point)
else:
rospy.logwarn("Failed to find ik solution for grasp waypoint %d" %
(i, ))
time_sec += time_incr
return trajectory
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 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 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 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 construct_joint_point(position, t):
point = JointTrajectoryPoint()
point.positions = position
if not isinstance(t, rospy.Duration):
t = rospy.Duration(t)
point.velocities = [0.0] * len(position)
point.accelerations = [0.0] * len(position)
point.time_from_start = t
return point
def trajectory_point(self, t, jointspace):
"""
takes a discrete point in time, and puts the position, velocity, and
acceleration into a ROS JointTrajectoryPoint() to be put into a
RobotTrajectory.
Parameters
----------
t : float
jointspace : bool
What kind of trajectory. Joint space points are 7x' and describe the
angle of each arm. Workspace points are 3x', and describe the x,y,z
position of the end effector.
Returns
-------
:obj:`trajectory_msgs.msg.JointTrajectoryPoint`
"""
point = JointTrajectoryPoint()
delta_t = .01
if jointspace:
x_t, x_t_1, x_t_2 = None, None, None
ik_attempts = 0
theta_t_2 = self.get_ik(self.target_position(t-2*delta_t))
theta_t_1 = self.get_ik(self.target_position(t-delta_t))
theta_t = self.get_ik(self.target_position(t))
# print(self.target_position(t))
#theta_t = np.array(theta_t)
# print(theta_t)
# we said you shouldn't simply take a finite difference when creating
# the path, why do you think we're doing that here? cause you're mean
point.positions = theta_t
point.velocities = (theta_t - theta_t_1) / delta_t
point.accelerations = (theta_t - 2*theta_t_1 + theta_t_2) / (2*delta_t)
else:
point.positions = self.target_position(t)
point.velocities = self.target_velocity(t)
point.accelerations = self.target_acceleration(t)
point.time_from_start = rospy.Duration.from_sec(t)
return point
def set_position(self,goal): traj = JointTrajectory() traj_p = JointTrajectoryPoint() traj.joint_names = ["torso_lift_joint"] traj_p.positions = [goal] traj_p.velocities = [0.] traj_p.accelerations = [0.] traj_p.time_from_start = rospy.Duration(2.) traj.points.append(traj_p) self.pub.publish(traj)
def reverse_planning(self, plan):
rev_plan = RobotTrajectory()
rev_joint_traj = JointTrajectory()
rev_joint_traj.header = copy.deepcopy(plan.joint_trajectory.header)
rev_joint_traj.joint_names = copy.deepcopy(plan.joint_trajectory.joint_names)
new_points = []
points = plan.joint_trajectory.points # JointTrajectoryPoint[]
l = len(points)
'''
How to reverse the cartesian plan?
Reverse the order of trajectory point's joint values.
Change the sign of velocity and acceleration.
Change the accumulative time parameterization to follow
the reverse order time gap.
But you should put the current joint value to the first
trajectory point. If not, the planner will return error due to
joint deviation.
'''
for i in range(l):
point = JointTrajectoryPoint()
if i == 0:
point.positions = self.ur5.get_current_joint_values()
point.velocities = [0.0 for val in point.positions]
point.accelerations = [0.0 for val in point.positions]
point.time_from_start = rospy.Duration(0)
else:
point.positions = copy.deepcopy(points[l-1-i].positions)
point.velocities = [-value for value in points[l-1-i].velocities]
point.accelerations = [-value for value in points[l-1-i].accelerations]
point.time_from_start = new_points[i-1].time_from_start + points[l-i].time_from_start - points[l-1-i].time_from_start
new_points.append(point)
rev_joint_traj.points = new_points
rev_plan.joint_trajectory = rev_joint_traj
return rev_plan
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 point():
global joy_r_v_avg
global joy_r_h_avg
global commanded_elev
global commanded_azim
rospy.init_node('manual_arm_control')
rate = rospy.Rate(50)
pub = rospy.Publisher('/arm_twist', JointTrajectoryPoint, queue_size=1)
rospy.Subscriber('/arm_start_stop', Bool, run_cb)
rospy.Subscriber("/joystick", Joy, joy_cb)
MAX_AZIM = rospy.get_param("/max_azimuth", np.pi * 2)
MIN_AZIM = rospy.get_param("/min_azimuth", -np.pi * 2)
MAX_ELEV = rospy.get_param("/max_elevation", np.pi / 4)
MIN_ELEV = rospy.get_param("/min_elevation", -np.pi / 4)
AZIM_MAX_SPEED = rospy.get_param("/max_azimuth_speed", np.pi / 2)
ELEV_MAX_SPEED = rospy.get_param("/max_elev_speed", np.pi / 16)
while not rospy.is_shutdown():
if run_var == True:
commanded_elev_speed = joy_r_v_avg * ELEV_MAX_SPEED
commanded_elev += commanded_elev_speed * dt
commanded_azim_speed = joy_r_h_avg * AZIM_MAX_SPEED
if commanded_azim_speed > 0:
commanded_azim = 100
else:
commanded_azim = -100
if commanded_azim > MAX_AZIM:
commanded_azim = MAX_AZIM
#commanded_azim_speed = 0
elif commanded_azim < MIN_AZIM:
commanded_azim = MIN_AZIM
#commanded_azim_speed = 0
if commanded_elev > MAX_ELEV:
commanded_elev = MAX_ELEV
#commanded_elev_speed = 0
elif commanded_elev < MIN_ELEV:
commanded_elev = MIN_ELEV
#commanded_elev_speed = 0
p = JointTrajectoryPoint()
azim = commanded_azim
elev = commanded_elev
azim_speed = commanded_azim_speed
elev_speed = commanded_elev_speed
p.positions = [commanded_azim, commanded_elev]
p.velocities = [commanded_azim_speed * 8, commanded_elev_speed * 2]
p.accelerations = [0, 0]
#p.time_from_start = rospy.Time.now()
pub.publish(p)
rate.sleep()
'''elif run_var == False:''' #TODO Set zero to current joint positions to avoid jumps
def execute_cb(self, goal):
arm_goal = FollowJointTrajectoryGoal()
arm_goal.goal_time_tolerance = rospy.Time(1.0)
arm_goal.trajectory.joint_names = [
'arm_joint_1', 'arm_joint_2', 'arm_joint_3', 'arm_joint_4',
'arm_joint_5'
]
for pt in goal.trajectory.points:
pt_temp = JointTrajectoryPoint()
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
arm_goal.trajectory.points.append(pt_temp)
base_goal = FollowJointTrajectoryGoal()
base_goal.goal_time_tolerance = rospy.Time(1.0)
base_goal.trajectory.joint_names = [
'virtual_x', 'virtual_y', 'virtual_theta'
]
for pt in goal.trajectory.points:
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.velocities.append(pt.velocities[6])
pt_temp.velocities.append(pt.velocities[7])
pt_temp.velocities.append(pt.velocities[5])
pt_temp.accelerations.append(pt.accelerations[6])
pt_temp.accelerations.append(pt.accelerations[7])
pt_temp.accelerations.append(pt.accelerations[5])
pt_temp.time_from_start = pt.time_from_start
base_goal.trajectory.points.append(pt_temp)
#rospy.loginfo(base_goal.trajectory.points[0].positions) #list of positions of one point
#rospy.loginfo(base_goal.trajectory.points[1].positions)
#rospy.loginfo(base_goal.trajectory.points[2].positions)
if self._as.is_preempt_requested():
rospy.loginfo("This action has been preempted")
self._as.set_preempted()
success = False
self._as.set_cancelled()
else:
#rospy.loginfo(base_goal.trajectory.points)
self.base_client.send_goal(base_goal)
self.arm_client.send_goal_and_wait(arm_goal)
success = True
if success:
rospy.loginfo('Action Succeeded')
self._as.set_succeeded()
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 create_JTG(self, arm, pos_arr, dur_arr, stamp=None, vel_arr=None, acc_arr=None):
# Hack
vel_arr = [[0.]*7]*len(pos_arr)
acc_arr = [[0.]*7]*len(pos_arr)
##
# Compute joint velocities and acclereations.
def get_vel_acc(q_arr, d_arr):
vel_arr = [[0.]*7]
acc_arr = [[0.]*7]
for i in range(1, len(q_arr)):
vel, acc = [], []
for j in range(7):
vel += [(q_arr[i][j] - q_arr[i-1][j]) / d_arr[i]]
acc += [(vel[j] - vel_arr[i-1][j]) / d_arr[i]]
vel_arr += [vel]
acc_arr += [acc]
print vel, acc
return vel_arr, acc_arr
if arm != 1:
arm = 0
jtg = JointTrajectoryGoal()
if stamp is None:
stamp = rospy.Time.now()
else:
jtg.trajectory.header.stamp = stamp
if len(pos_arr) > 1 and (vel_arr is None or acc_arr is None):
v_arr, a_arr = get_vel_acc(pos_arr, dur_arr)
if vel_arr is None:
vel_arr = v_arr
if acc_arr is None:
acc_arr = a_arr
jtg.trajectory.joint_names = self.joint_names_list[arm]
for i in range(len(pos_arr)):
if pos_arr[i] is None or type(pos_arr[i]) is types.NoneType:
continue
jtp = JointTrajectoryPoint()
jtp.positions = pos_arr[i]
if vel_arr is None:
vel = [0.] * 7
else:
vel = vel_arr[i]
jtp.velocities = vel
if acc_arr is None:
acc = [0.] * 7
else:
acc = acc_arr[i]
jtp.accelerations = acc
jtp.time_from_start = rospy.Duration(dur_arr[i])
jtg.trajectory.points.append(jtp)
return jtg
def cmd_pub(self):
point_goal = JointTrajectoryPoint()
point_goal.positions = [0.4, -0.5]
point_goal.velocities = [0, 0]
point_goal.accelerations = [0, 0]
point_goal.time_from_start = rospy.Time(secs=2, nsecs=0)
self.action_goal.trajectory.points.append(point_goal)
self.action_client.wait_for_server()
self.action_client.send_goal(self.action_goal)
self.action_goal.trajectory.points = []
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 createArmTrajectory(self, values):
traj_msg = JointTrajectory()
traj_msg.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0)
traj_msg.joint_names = self.GetJointNames()
traj_msg.points = []
end_point = JointTrajectoryPoint()
end_point.positions = values
end_point.velocities = [0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
end_point.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
end_point.time_from_start = rospy.Duration.from_sec(1.0)
traj_msg.points.append(end_point)
return traj_msg
def stop(self, include_velocity=False, include_effort=False):
"""
Stop the recording and returns the recorded trajectory of joints declared in the constructor
:param include_velocity: if True, the joint velocities will be included in the returned trajectory
:param include_effort: if True, the joint efforts will be included in the returned trajectory
:return: (joint_traj, eef_traj) resp. the recorded joint trajectory (RobotTrajectory) and end effector trajectory in frame 'base' (Path)
"""
with self._recording_lock:
self._recording = False
# Now we reconstruct a RobotTrajectory...
rt = RobotTrajectory()
rt.joint_trajectory.joint_names = self._joints
with self._joint_recorder_lock:
for idx, js in enumerate(self._joint_recorded_points):
jtp = JointTrajectoryPoint()
try:
jtp.positions = [
js.position[js.name.index(joint)]
for joint in rt.joint_trajectory.joint_names
]
except ValueError as e:
# This specific JS sample does not contain the desired joints, skip it
continue
else:
if include_velocity:
jtp.velocities = [
js.velocity[js.name.index(joint)]
for joint in rt.joint_trajectory.joint_names
]
if include_effort:
jtp.accelerations = [
js.effort[js.name.index(joint)]
for joint in rt.joint_trajectory.joint_names
]
jtp.time_from_start = self._joint_recorded_times[
idx] - self._joint_recorded_times[0]
rt.joint_trajectory.points.append(jtp)
# ... as well as an End Effector trajectory
eft = Path()
with self._eef_recorder_lock:
for eef in self._eef_recorded_points:
ps = PoseStamped()
ps.header = eef.header
ps.header.frame_id = 'base'
ps.pose = eef.pose
eft.poses.append(ps)
# twist and wrench in eef are discarded
eft.header.frame_id = 'base'
return rt, eft
def send_left_arm_goal(self, j_positions):
jt = JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.joint_names = ['l_upper_arm_roll_joint', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint',
'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint']
jtp = JointTrajectoryPoint()
jtp.positions = list(j_positions)
jtp.velocities = [0.0] * len(j_positions)
jtp.accelerations = [0.0] * len(j_positions)
jtp.time_from_start = rospy.Duration(0.45)
jt.points.append(jtp)
self.left_command.publish(jt)
def trapezoid_gen(target, current_joint_angles, duration):
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
]
goal.trajectory.header.frame_id = '/world'
dist = target - current_joint_angles
vmax = 1.5 * dist / duration
acc = 3 * vmax / duration
p1 = JointTrajectoryPoint()
p1.positions = current_joint_angles
p1.velocities = np.zeros((6, ))
p1.accelerations = acc
p1.time_from_start = rospy.Duration(0)
p2 = JointTrajectoryPoint()
p2.positions = np.array(p1.positions) + dist / 4
p2.velocities = vmax
p2.accelerations = np.zeros((6, ))
p2.time_from_start = rospy.Duration(duration / 3)
p3 = JointTrajectoryPoint()
p3.positions = np.array(p1.positions) + 3 * dist / 4
p3.velocities = vmax
p3.accelerations = -acc
p3.time_from_start = rospy.Duration(duration * 2 / 3)
p4 = JointTrajectoryPoint()
p4.positions = target
p4.velocities = np.zeros((6, ))
p4.accelerations = np.zeros((6, ))
p4.time_from_start = rospy.Duration(duration)
goal.trajectory.points.append(p1)
goal.trajectory.points.append(p2)
goal.trajectory.points.append(p3)
goal.trajectory.points.append(p4)
return goal
def set_jointangles(self, arm, q, duration=0.15):
rospy.logwarn('Currently ignoring the arm parameter.')
# for i,p in enumerate(self.r_arm_pub_l):
# p.publish(q[i])
jtg = JointTrajectoryGoal()
jtg.trajectory.joint_names = self.joint_names_list
jtp = JointTrajectoryPoint()
jtp.positions = q
jtp.velocities = [0 for i in range(len(q))]
jtp.accelerations = [0 for i in range(len(q))]
jtp.time_from_start = rospy.Duration(duration)
jtg.trajectory.points.append(jtp)
self.joint_action_client.send_goal(jtg)
def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
if not self._alive:
self._limb.exit_control_mode()
elif self._mode == 'joint_impedance':
pnt = JointTrajectoryPoint()
pnt.positions = self._get_current_position(joint_names)
if self._mode == 'joint_impedance' and self._alive:
# zero inverse dynamics feedforward command
pnt.velocities = [0.0] * len(joint_names)
pnt.accelerations = [0.0] * len(joint_names)
self._limb.cmd_joint_angles(pnt.positions,
pnt.velocities,
pnt.accelerations)
def set_jointangles(self, arm, q, duration=0.15):
rospy.logwarn('Currently ignoring the arm parameter.')
# for i,p in enumerate(self.r_arm_pub_l):
# p.publish(q[i])
jtg = JointTrajectoryGoal()
jtg.trajectory.joint_names = self.joint_names_list
jtp = JointTrajectoryPoint()
jtp.positions = q
jtp.velocities = [0 for i in range(len(q))]
jtp.accelerations = [0 for i in range(len(q))]
jtp.time_from_start = rospy.Duration(duration)
jtg.trajectory.points.append(jtp)
self.joint_action_client.send_goal(jtg)
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 scale_trajectory_speed(traj, scale):
# Create a new trajectory object
new_traj = RobotTrajectory()
# Initialize the new trajectory to be the same as the input trajectory
new_traj.joint_trajectory = traj.joint_trajectory
# Get the number of joints involved
n_joints = len(traj.joint_trajectory.joint_names)
# Get the number of points on the trajectory
n_points = len(traj.joint_trajectory.points)
# Store the trajectory points
points = list(traj.joint_trajectory.points)
# Cycle through all points and joints and scale the time from start,
# as well as joint speed and acceleration
for i in range(n_points):
point = JointTrajectoryPoint()
# The joint positions are not scaled so pull them out first
point.positions = traj.joint_trajectory.points[i].positions
# Next, scale the time_from_start for this point
point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
# Get the joint velocities for this point
point.velocities = list(traj.joint_trajectory.points[i].velocities)
# Get the joint accelerations for this point
point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
# Scale the velocity and acceleration for each joint at this point
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * scale
point.accelerations[j] = point.accelerations[j] * scale * scale
# Store the scaled trajectory point
points[i] = point
# Assign the modified points to the new trajectory
new_traj.joint_trajectory.points = points
# Return the new trajecotry
return new_traj
def createGoalWithValueAndPublish(self, value):
jt = JointTrajectory()
jt.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
# Get current values and modify the one we want
names, values = self.getNamesAndMsgListAndModifyValue(self.joint_names,
self.joint_name,
value)
jtp.positions = values
jtp.velocities = [0.0] * len(self.joint_names)
jtp.accelerations = [0.0] * len(self.joint_names)
# TODO: Make duration dependent on how big is the movement
scaled_time = abs(value) * 1.5 + 0.2
jtp.time_from_start = rospy.Duration(scaled_time)
jt.points.append(jtp)
self.pub.publish(jt)
rospy.sleep(1.0)
def receiverLoop(self):
while True:
try:
input_msg = pickle.load( open( "/dev/shm/right_arm_goal.p", "rb" ) )
os.remove("/dev/shm/right_arm_goal.p")
except IOError:
print "file not there..."
rospy.sleep(0.1)
continue
print input_msg
goal = FollowJointTrajectoryGoal()
rospy.loginfo("input_msg.goal.goal_time_tolerance: \n" + str(input_msg.goal.goal_time_tolerance ) )
goal.goal_time_tolerance = rospy.Duration(secs=0, nsecs=0)
rospy.loginfo("input_msg.goal.goal_tolerance: \n" + str(input_msg.goal.goal_tolerance ) )
goal.goal_tolerance = input_msg.goal.goal_tolerance
rospy.loginfo("input_msg.goal.path_tolerance: \n" + str(input_msg.goal.path_tolerance ) )
goal.path_tolerance = input_msg.goal.path_tolerance
rospy.loginfo("input_msg.goal.trajectory.header: \n" + str(input_msg.goal.trajectory.header ) )
goal.trajectory.header = input_msg.goal.trajectory.header
goal.trajectory.header.frame_id = '/base_link'
goal.trajectory.joint_names = input_msg.goal.trajectory.joint_names
input_times = pickle.load( open( "/dev/shm/right_arm_goal_times.p", "rb" ) )
rospy.loginfo("input_times is:\n" + str(input_times))
for input_point, time in zip(input_msg.goal.trajectory.points, input_times):
p = JointTrajectoryPoint()
p.accelerations = input_point.accelerations
p.positions = input_point.positions
p.velocities = input_point.velocities
# rospy.loginfo("???????????????????????????????????????????????????????????????")
# rospy.loginfo("Time from start is:\n" + str(input_point.time_from_start))
# rospy.loginfo("time is:\n" + str(time))
# rospy.loginfo("???????????????????????????????????????????????????????????????")
p.time_from_start = time
goal.trajectory.points.append(p)
os.remove("/dev/shm/right_arm_goal_times.p")
#print "Goal will be:"
rospy.loginfo("!!!!!!!!!!!Goal:\n" +str(goal))
self.right_arm_as.send_goal(goal)
rospy.loginfo("Waiting for result")
self.right_arm_as.wait_for_result()
rospy.loginfo("Goal done")
def move_to(self, positions, velocities, accelerations, delta_t):
assert(len(self.joint_names) == positions.shape[1])
trajectory = JointTrajectory()
trajectory.joint_names = self.joint_names
for i in range(positions.shape[0]): # for each row i
p = JointTrajectoryPoint()
p.positions = list(positions[i])
p.velocities = list(velocities[i])
p.accelerations = list(accelerations[i])
p.time_from_start = rospy.Duration(i * delta_t)
trajectory.points.append(p)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
self.client.send_goal(follow_goal)
self.client.wait_for_result()
def stop(self, include_velocity=False, include_effort=False):
"""
Stop the recording and returns the recorded trajectory of joints declared in the constructor
:param include_velocity: if True, the joint velocities will be included in the returned trajectory
:param include_effort: if True, the joint efforts will be included in the returned trajectory
:return: (joint_traj, eef_traj) resp. the recorded joint trajectory (RobotTrajectory) and end effector trajectory in frame 'base' (Path)
"""
with self._recording_lock:
self._recording = False
# Now we reconstruct a RobotTrajectory...
rt = RobotTrajectory()
rt.joint_trajectory.joint_names = self._joints
with self._joint_recorder_lock:
for idx, js in enumerate(self._joint_recorded_points):
jtp = JointTrajectoryPoint()
try:
jtp.positions = [js.position[js.name.index(joint)] for joint in rt.joint_trajectory.joint_names]
except ValueError as e:
# This specific JS sample does not contain the desired joints, skip it
continue
else:
if include_velocity:
jtp.velocities = [js.velocity[js.name.index(joint)] for joint in rt.joint_trajectory.joint_names]
if include_effort:
jtp.accelerations = [js.effort[js.name.index(joint)] for joint in rt.joint_trajectory.joint_names]
jtp.time_from_start = self._joint_recorded_times[idx] - self._joint_recorded_times[0]
rt.joint_trajectory.points.append(jtp)
# ... as well as an End Effector trajectory
eft = Path()
with self._eef_recorder_lock:
for eef in self._eef_recorded_points:
ps = PoseStamped()
ps.header = eef.header
ps.header.frame_id = 'base'
ps.pose = eef.pose
eft.poses.append(ps)
# twist and wrench in eef are discarded
eft.header.frame_id = 'base'
return rt, eft
def track_trajectory( pos, vel, acc, time,
arm = 'r', client = None,
stamp = None):
if (acc == None):
acc = np.zeros( pos.shape )
if ( len( pos ) != len( vel ) or len( pos ) != len( time ) ):
rospy.logerr( '[track_trajectory] dimensions do not agree' )
rospy.loginfo( 'arm \'%s\' tracking trajectory with %d points'
%( arm, len( pos ) ) )
if (client == None):
client = init_jt_client(arm)
else:
arm = client.action_client.ns[0:1]; # ignore arm argument
joint_names = get_joint_names( ''.join( ( arm, '_arm_controller' ) ) )
# create trajectory message
jt = JointTrajectory()
jt.joint_names = joint_names
jt.points = []
t = 0
for i in range( len( pos ) ):
jp = JointTrajectoryPoint()
jp.time_from_start = rospy.Time.from_sec( time[i] )
jp.positions = pos[i,:]
jp.velocities = vel[i,:]
jp.accelerations = acc[i,:]
jt.points.append( jp )
t += 1
if ( stamp == None ):
stamp = rospy.Time.now()
# push trajectory goal to ActionServer
jt_goal = JointTrajectoryGoal()
jt_goal.trajectory = jt
jt_goal.trajectory.header.stamp = stamp;
client.send_goal(jt_goal)
def spam_controller(controller_name, joint_names):
cmd_topic = "/" + controller_name + "/command"
rospy.loginfo("Going to spam controller: " + controller_name +
"\n in command topic: " + cmd_topic +
"\n with joints: " + str(joint_names) )
ctl_pub = rospy.Publisher(cmd_topic, JointTrajectory)
rospy.sleep(0.1)
while not rospy.is_shutdown():
jt = JointTrajectory()
jt.joint_names = joint_names
jtp = JointTrajectoryPoint()
jtp.positions = get_valid_random_values(joint_names, joints_dict)
jtp.velocities = [0.0] * len(joint_names)
jtp.accelerations = [0.0] * len(joint_names)
jtp.time_from_start = rospy.Duration(random.random() + 0.5) # 0.5s to 1.5s
jt.points.append(jtp)
ctl_pub.publish(jt)
rospy.logdebug("Sent to: " + cmd_topic + "\n" + str(jt))
rospy.sleep(jtp.time_from_start)
def _get_bezier_point(self, b_matrix, idx, t, cmd_time, dimensions_dict):
pnt = JointTrajectoryPoint()
pnt.time_from_start = rospy.Duration(cmd_time)
num_joints = b_matrix.shape[0]
pnt.positions = [0.0] * num_joints
if dimensions_dict["velocities"]:
pnt.velocities = [0.0] * num_joints
if dimensions_dict["accelerations"]:
pnt.accelerations = [0.0] * num_joints
for jnt in range(num_joints):
b_point = bezier.bezier_point(b_matrix[jnt, :, :, :], idx, t)
# Positions at specified time
pnt.positions[jnt] = b_point[0]
# Velocities at specified time
if dimensions_dict["velocities"]:
pnt.velocities[jnt] = b_point[1]
# Accelerations at specified time
if dimensions_dict["accelerations"]:
pnt.accelerations[jnt] = b_point[-1]
return pnt
def _get_point(self, joint_names, time):
pnt = JointTrajectoryPoint()
pnt.time_from_start = rospy.Duration(time)
pnt.positions = [0.0] * len(joint_names)
pnt.velocities = [0.0] * len(joint_names)
pnt.accelerations = [0.0] * len(joint_names)
time2 = math.pow(time, 2)
time3 = math.pow(time, 3)
time4 = math.pow(time, 4)
time5 = math.pow(time, 5)
for jnt in xrange(len(joint_names)):
# Positions at specified time
pnt.positions[jnt] = (self._coeff[jnt][0]
+ time * self._coeff[jnt][1]
+ time2 * self._coeff[jnt][2]
+ time3 * self._coeff[jnt][3]
+ time4 * self._coeff[jnt][4]
+ time5 * self._coeff[jnt][5]
)
# Velocities at specified time
pnt.velocities[jnt] = (self._coeff[jnt][1]
+ 2.0 * time * self._coeff[jnt][2]
+ (3.0 * time2
* self._coeff[jnt][3])
+ (4.0 * time3
* self._coeff[jnt][4])
+ (5.0 * time4
* self._coeff[jnt][5])
)
# Accelerations at specified time
pnt.accelerations[jnt] = (2.0 * self._coeff[jnt][2]
+ 6.0 * time * self._coeff[jnt][3]
+ (12.0 * time2
* self._coeff[jnt][4])
+ (20.0 * time3
* self._coeff[jnt][5])
)
return pnt
def receiverLoop(self):
while True:
print "self.conn.recv()"
input_msg = self.conn.recv()
print input_msg
goal = FollowJointTrajectoryGoal()
goal.goal_time_tolerance = input_msg.goal_time_tolerance
goal.goal_tolerance = input_msg.goal_tolerance
goal.path_tolerance = input_msg.path_tolerance
goal.trajectory.header = input_msg.trajectory.header
goal.trajectory.joint_names = input_msg.joint_names
for input_point in input_msg.trajectory.points:
p = JointTrajectoryPoint()
p.accelerations = input_point.accelerations
p.positions = input_point.positions
p.velocities = input_point.velocities
p.time_from_start = input_point.time_from_start
goal.trajectory.points.append(p)
print "Goal will be:"
print goal
#self.right_arm_as.send_goal(goal)
rospy.loginfo("Waiting for result")
self.right_arm_as.wait_for_result()
rospy.loginfo("Goal done")
pos = pos.split(", ")
for i in range(0, len(pos)):
pos[i] = float(pos[i])
y.positions = pos
#get vel
vel = el[counter + 2].split(": ")[1][1:-1]
vel = vel.split(", ")
for i in range(0, len(vel)):
vel[i] = float(vel[i])
y.velocities = vel
#get acc
acc = el[counter + 3].split(": ")[1][1:-1]
acc = acc.split(", ")
for i in range(0, len(acc)):
acc[i] = float(acc[i])
y.accelerations = acc
#get time
st = el[counter + 6].split(": ")[1]
et = el[counter + 7].split(": ")[1]
y.time_from_start.secs = float(st)
y.time_from_start.nsecs = float(et)
x.joint_trajectory.points.append(y)
counter = counter + 8
acHan.execute_plan(x)
t.close()
def _on_trajectory_action(self, goal):
joint_names = goal.trajectory.joint_names
trajectory_points = goal.trajectory.points
# Load parameters for trajectory
self._get_trajectory_parameters(joint_names, goal)
# Create a new discretized joint trajectory
num_points = len(trajectory_points)
if num_points == 0:
rospy.logerr("%s: Empty Trajectory" % (self._action_name,))
self._server.set_aborted()
return
rospy.loginfo("%s: Executing requested joint trajectory" % (self._action_name,))
rospy.logdebug("Trajectory Points: {0}".format(trajectory_points))
control_rate = rospy.Rate(self._control_rate)
dimensions_dict = self._determine_dimensions(trajectory_points)
if num_points == 1:
# Add current position as trajectory point
first_trajectory_point = JointTrajectoryPoint()
first_trajectory_point.positions = self._get_current_position(joint_names)
# To preserve desired velocities and accelerations, copy them to the first
# trajectory point if the trajectory is only 1 point.
if dimensions_dict["velocities"]:
first_trajectory_point.velocities = deepcopy(trajectory_points[0].velocities)
if dimensions_dict["accelerations"]:
first_trajectory_point.accelerations = deepcopy(trajectory_points[0].accelerations)
first_trajectory_point.time_from_start = rospy.Duration(0)
trajectory_points.insert(0, first_trajectory_point)
num_points = len(trajectory_points)
# Force Velocites/Accelerations to zero at the final timestep
# if they exist in the trajectory
# Remove this behavior if you are stringing together trajectories,
# and want continuous, non-zero velocities/accelerations between
# trajectories
if dimensions_dict["velocities"]:
trajectory_points[-1].velocities = [0.0] * len(joint_names)
if dimensions_dict["accelerations"]:
trajectory_points[-1].accelerations = [0.0] * len(joint_names)
# Compute Full Bezier Curve Coefficients for all 7 joints
pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
try:
b_matrix = self._compute_bezier_coeff(joint_names, trajectory_points, dimensions_dict)
except Exception as ex:
rospy.logerr(
("{0}: Failed to compute a Bezier trajectory for {1}" ' arm with error "{2}: {3}"').format(
self._action_name, self._name, type(ex).__name__, ex
)
)
self._server.set_aborted()
return
# Wait for the specified execution time, if not provided use now
start_time = goal.trajectory.header.stamp.to_sec()
if start_time == 0.0:
start_time = rospy.get_time()
baxter_dataflow.wait_for(lambda: rospy.get_time() >= start_time, timeout=float("inf"))
# Loop until end of trajectory time. Provide a single time step
# of the control rate past the end to ensure we get to the end.
# Keep track of current indices for spline segment generation
now_from_start = rospy.get_time() - start_time
end_time = trajectory_points[-1].time_from_start.to_sec()
while now_from_start < end_time and not rospy.is_shutdown():
# Acquire Mutex
now = rospy.get_time()
now_from_start = now - start_time
idx = bisect.bisect(pnt_times, now_from_start)
# Calculate percentage of time passed in this interval
if idx >= num_points:
cmd_time = now_from_start - pnt_times[-1]
t = 1.0
elif idx >= 0:
cmd_time = now_from_start - pnt_times[idx - 1]
t = cmd_time / (pnt_times[idx] - pnt_times[idx - 1])
else:
cmd_time = 0
t = 0
point = self._get_bezier_point(b_matrix, idx, t, cmd_time, dimensions_dict)
# Command Joint Position, Velocity, Acceleration
command_executed = self._command_joints(joint_names, point, start_time, dimensions_dict)
self._update_feedback(deepcopy(point), joint_names, now_from_start)
# Release the Mutex
if not command_executed:
return
control_rate.sleep()
# Keep trying to meet goal until goal_time constraint expired
last = trajectory_points[-1]
last_time = trajectory_points[-1].time_from_start.to_sec()
end_angles = dict(zip(joint_names, last.positions))
def check_goal_state():
for error in self._get_current_error(joint_names, last.positions):
if self._goal_error[error[0]] > 0 and self._goal_error[error[0]] < math.fabs(error[1]):
return error[0]
if (
self._stopped_velocity > 0.0
and max([abs(cur_vel) for cur_vel in self._get_current_velocities(joint_names)])
> self._stopped_velocity
):
return False
else:
return True
while now_from_start < (last_time + self._goal_time) and not rospy.is_shutdown():
if not self._command_joints(joint_names, last, start_time, dimensions_dict):
return
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names, now_from_start)
control_rate.sleep()
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names, now_from_start)
# Verify goal constraint
result = check_goal_state()
if result is True:
rospy.loginfo("%s: Joint Trajectory Action Succeeded for %s arm" % (self._action_name, self._name))
self._result.error_code = self._result.SUCCESSFUL
self._server.set_succeeded(self._result)
elif result is False:
rospy.logerr("%s: Exceeded Max Goal Velocity Threshold for %s arm" % (self._action_name, self._name))
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
else:
rospy.logerr("%s: Exceeded Goal Threshold Error %s for %s arm" % (self._action_name, result, self._name))
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
self._command_stop(goal.trajectory.joint_names, end_angles, start_time, dimensions_dict)
def spin(self):
r = rospy.Rate(1.0/self.dt)
while not rospy.is_shutdown():
if self.req_pos is None:
r.sleep()
continue
self.lock.acquire()
req_vel = copy.deepcopy(self.joint_req_vel)
last_vel_command = copy.deepcopy(self.last_vel_command)
act_position = copy.deepcopy(self.act_position)
#req_pos = copy.deepcopy(self.req_pos)
now = rospy.Time.now()
if last_vel_command is None:
self.lock.release()
r.sleep()
continue
if (now - last_vel_command) > rospy.Duration(0.5):
print "final positions"
print act_position.positions
self.last_vel_command = None
self.lock.release()
r.sleep()
continue
jt = JointTrajectory()
jt.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
jtp.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jtp.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jtp.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
#tl = list(self.req_pos.positions)
for idx in range(0,len(req_vel)):
self.req_pos[idx] = self.req_pos[idx] + (req_vel[idx]*self.dt)
jtp.positions[idx] = self.req_pos[idx]
jtp.velocities[idx] = req_vel[idx]
#jtp.accelerations[idx] = 150.0
jt.header.stamp = now
jt.points.append(jtp)
self.lock.release()
self.pub.publish(jt)
r.sleep()
cur_names, cur_pos = zip(*cur_state) # unzip end_pos = [] for p in cur_pos: end_pos.append(p + 0.2) traj_msg = JointTrajectory() traj_msg.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(2.0) traj_msg.joint_names = cur_names traj_msg.points = [] orig_point = JointTrajectoryPoint() orig_point.positions = cur_pos orig_point.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] orig_point.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] orig_point.time_from_start = rospy.Duration.from_sec(0.0) traj_msg.points.append(orig_point) end_point = JointTrajectoryPoint() end_point.positions = end_pos end_point.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] end_point.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] end_point.time_from_start = rospy.Duration.from_sec(2.0) traj_msg.points.append(end_point) print traj_msg traj_f = client.execute(traj_msg) print traj_f.result()
'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 'arm_left_7_joint']
CONTROLLER_CMD_TOPIC = "/left_arm_controller/command"
def get_n_randoms(n, min, max):
randoms = []
max_value = max - min
for i in range(n):
randoms.append((random.random() * max_value) + min) # this scales the value between min and max
return randoms
if __name__ == '__main__':
rospy.init_node('random_left_arm_goals')
ctl_pub = rospy.Publisher(CONTROLLER_CMD_TOPIC, JointTrajectory)
rospy.sleep(0.1)
while not rospy.is_shutdown():
jt = JointTrajectory()
jt.joint_names = JOINT_NAMES
jtp = JointTrajectoryPoint()
jtp.positions = get_n_randoms(len(JOINT_NAMES), -1.0, 4.0)
jtp.velocities = [0.0] * len(JOINT_NAMES)
jtp.accelerations = [0.0] * len(JOINT_NAMES)
jtp.time_from_start = rospy.Duration(random.random() + 0.5) # 0.5s to 1.5s
jt.points.append(jtp)
ctl_pub.publish(jt)
rospy.loginfo("Sent: " + str(jt))
rospy.sleep(jtp.time_from_start)
def getPlans(f, acHan):
file_plans = []
plans = f.read().split("PLAN : ")
plans = plans[1:]
for p in plans:
name_plan = p.split(' >\n')
#print(name_plan[0])
el = name_plan[1].split('\n')
if (len(el) < 5):
file_plans.append(el[0])
else:
x = RobotTrajectory()
el = p.split('\n')
#get frame id
fram_id = el[7].split(": ")[1]
x.joint_trajectory.header.frame_id = fram_id
#get joint names
joints = el[8].split(": ")[1][1:-1]
joints = joints.split(", ")
for j in range(0, len(joints)):
joints[j] = joints[j].split('\'')[1]
x.joint_trajectory.joint_names = joints
#get points
done = False
counter = 10
while(not done):
if ("multi" in el[counter]):
done = True
else:
y = JointTrajectoryPoint()
#get pos
pos = el[counter + 1].split(": ")[1][1:-1]
pos = pos.split(", ")
for i in range(0, len(pos)):
pos[i] = float(pos[i])
y.positions = pos
#get vel
vel = el[counter + 2].split(": ")[1][1:-1]
vel = vel.split(", ")
for i in range(0, len(vel)):
vel[i] = float(vel[i])
y.velocities = vel
#get acc
acc = el[counter + 3].split(": ")[1][1:-1]
acc = acc.split(", ")
for i in range(0, len(acc)):
acc[i] = float(acc[i])
y.accelerations = acc
#get time
st = el[counter + 6].split(": ")[1]
et = el[counter + 7].split(": ")[1]
y.time_from_start.secs = float(st)
y.time_from_start.nsecs = float(et)
x.joint_trajectory.points.append(y)
counter = counter + 8
file_plans.append(x)
return file_plans
def makePoint(position):
mypoint = JointTrajectoryPoint()
mypoint.positions = position
mypoint.velocities = [ 0, 0, 0, 0, 0, 0, 0 ]
mypoint.accelerations = [ 0, 0, 0, 0, 0, 0, 0 ]
return mypoint
