def generate_movement(path):
"""
Generate Crustcrawler arm movement through a message
:param path: List of points to draw
:returns: FollowJointTrajectoryGoal describing the arm movement
"""
# Generate our goal message
movement = FollowJointTrajectoryGoal()
# Names describes which joint is actuated by which element in the coming
# matrices
movement.trajectory.joint_names.extend(["joint_1", "joint_2", "joint_3"])
# Goal tolerance describes how much we allow the movement to deviate
# from true value at the end
movement.goal_tolerance.extend(
[
JointTolerance("joint_1", 0.1, 0.0, 0.0),
JointTolerance("joint_2", 0.1, 0.0, 0.0),
JointTolerance("joint_3", 0.1, 0.0, 0.0),
]
)
# Goal time is how many seconds we allow the movement to take beyond
# what we define in the trajectory
movement.goal_time_tolerance = rospy.Duration(2) # seconds
time = 4.0 # Cumulative time since start in seconds
# start_pos = inverse_kinematic([0.0, 0.0, np.pi / 2.0])
movement.trajectory.points.append(
create_trajectory_point([0.0, 0.0, np.pi / 2.0], time)
)
# First point in path will require extra time
time += 12.0
movement.trajectory.points.append(
create_trajectory_point(inverse_kinematic(path[0]), time)
)
# The rest of the points
for i_point in range(1, len(path)):
prev_point = path[i_point - 1]
point = path[i_point]
delta = [
point[0] - prev_point[0],
point[1] - prev_point[1],
point[2] - prev_point[2],
]
dist = np.sqrt(delta[0] ** 2 + delta[1] ** 2 + delta[2] ** 2)
time += dist
print("Point (deltatime, coordinates, joint rotations):")
print(dist)
print(point)
print(np.degrees(inverse_kinematic(point)))
print("---------------------------------------")
movement.trajectory.points.append(
create_trajectory_point(inverse_kinematic(point), time)
)
# Once drawing is done we add the default position
time += 8.0
movement.trajectory.points.append(
create_trajectory_point([0.0, 0.0, np.pi / 2.0], time)
)
return movement
def createHandGoal(side, j1, j2, j3):
"""Creates a FollowJointTrajectoryGoal with the values specified in j1, j2 and j3 for the joint positions
with the hand specified in side
@arg side string 'right' or 'left'
@arg j1 float value for joint 'hand_'+side+'_thumb_joint'
@arg j2 float value for joint 'hand_'+side+'_middle_joint'
@arg j3 float value for joint 'hand_'+side+'_index_joint'
@return FollowJointTrajectoryGoal with the specified goal"""
fjtg = FollowJointTrajectoryGoal()
fjtg.trajectory.joint_names.append('hand_'+side+'_thumb_joint')
fjtg.trajectory.joint_names.append('hand_'+side+'_middle_joint')
fjtg.trajectory.joint_names.append('hand_'+side+'_index_joint')
point = JointTrajectoryPoint()
point.positions.append(j1)
point.positions.append(j2)
point.positions.append(j3)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.time_from_start = rospy.Duration(4.0)
fjtg.trajectory.points.append(point)
for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
fjtg.goal_tolerance.append(goal_tol)
fjtg.goal_time_tolerance = rospy.Duration(3)
fjtg.trajectory.header.stamp = rospy.Time.now()
return fjtg
def __init__(self):
self.time = 0.
self.point_counter = 0
self.max_messages = INT_MAX
self.movement = FollowJointTrajectoryGoal()
self.movement.trajectory.joint_names.extend(['joint_1', 'joint_2', 'joint_3'])
self.movement.goal_tolerance.extend([ JointTolerance('joint_1', 0.1, 0., 0.), JointTolerance('joint_2', 0.1, 0., 0.), JointTolerance('joint_3', 0.1, 0., 0.) ])
self.movement.goal_time_tolerance = rospy.Duration(0.5)
def build_follow_trajectory(self, traj):
# Build 'Follow Joint Trajectory' goal from trajectory (includes tolerances for error)
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = traj
tolerance = JointTolerance()
tolerance.position = 0.05
tolerance.velocity = 0.1
traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_time_tolerance = rospy.Duration(3)
return traj_goal
def build_follow_trajectory(self, traj):
# Build 'Follow Joint Trajectory' goal from trajectory (includes tolerances for error)
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = traj
tolerance = JointTolerance()
tolerance.position = 0.05
tolerance.velocity = 0.1
traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_time_tolerance = rospy.Duration(3)
return traj_goal
def __init__(self):
# Action Client
self.move_client = SimpleActionClient(MOVE_BASE_TOPIC, MoveBaseAction)
rospy.loginfo("BASE CLIENT: Waiting for base action server...")
base_client_running = self.move_client.wait_for_server(
rospy.Duration(3))
if base_client_running:
rospy.loginfo("BASE CLIENT: Base controller initialized.")
else:
rospy.loginfo("BASE CLIENT: Base controller is NOT initialized!")
# Omni Client
self.omni_client = SimpleActionClient(OMNI_BASE_CLIENT_TOPIC,
FollowJointTrajectoryAction)
omni_client_running = self.omni_client.wait_for_server(
rospy.Duration(3))
# Current Pose
rospy.Subscriber(CURRENT_POSE_TOPIC, PoseStamped, self._pose_cb)
self.current_pose = PoseStamped()
# Velocity
self.vel_pub = rospy.Publisher(BASE_VELOCITY_TOPIC,
Twist,
queue_size=10)
# Default move_base goal init
self.move_goal = MoveBaseGoal()
self.move_goal.target_pose.header.frame_id = 'map'
self.timeout = rospy.Duration(30)
# Default traj goal init
# Tolerances
self.tolx = JointTolerance(name='odom_x', position=0.1)
self.toly = JointTolerance(name='odom_y', position=0.1)
self.tolt = JointTolerance(name='odom_t', position=0.1)
self.traj_goal = FollowJointTrajectoryGoal()
self.traj_goal.goal_tolerance = [self.tolx, self.toly, self.tolt]
self.traj_goal.path_tolerance = [self.tolx, self.toly, self.tolt]
self.traj_goal.trajectory.joint_names = OMNI_BASE_JOINTS
self.traj_goal.trajectory.header.frame_id = 'map'
r = RosPack()
pkg_path = r.get_path('frasier_utilities')
self.yaml_filename = pkg_path + '/config/wrs_map.yaml'
with open(self.yaml_filename, 'r') as outfile:
try:
self.locations = load(outfile)
except YAMLError as e:
print e
def generate_movement(path):
"""
Generate Crustcrawler arm movement through a message
:param path: List of points to draw
:returns: FollowJointTrajectoryGoal describing the arm movement
"""
# Generate our goal message
movement = FollowJointTrajectoryGoal()
# Names describes which joint is actuated by which element in the coming
# matrices
movement.trajectory.joint_names.extend(['joint_1', 'joint_2', 'joint_3'])
# Goal tolerance describes how much we allow the movement to deviate
# from true value at the end
movement.goal_tolerance.extend([
JointTolerance('joint_1', 0.1, 0., 0.),
JointTolerance('joint_2', 0.1, 0., 0.),
JointTolerance('joint_3', 0.1, 0., 0.)
])
# Goal time is how many seconds we allow the movement to take beyond
# what we define in the trajectory
movement.goal_time_tolerance = rospy.Duration(0.5) # seconds
time = 4.0 # Cumulative time since start in seconds
movement.trajectory.points.append(
create_trajectory_point([0., 0., np.pi / 2.], time))
# Add initial point, also as a large time fraction to avoid jerking
time += 4.0
movement.trajectory.points.append(
create_trajectory_point(inverse_kinematic(path[0]), time))
# Calculate total circle length
length = path_length(path)
# Calculate how much time we have to process each point of the circle
time_delta = (length / 2.) / len(path)
for point in path[1:]:
time += time_delta
movement.trajectory.points.append(
create_trajectory_point(inverse_kinematic(point), time))
# Once drawing is done we add the default position
time += 4.0
movement.trajectory.points.append(
create_trajectory_point([0., 0., np.pi / 2.], time))
return movement
def __init__(self, arm_trajectory_action_name, robotiq_hand_command_topic,
robotiq_hand_state_topic, robotiq_hand_joints_topic,
arm_joint_names, hand_joint_names):
"""
Creates a new SynchedHandArmController.
"""
self._action_client = actionlib.SimpleActionClient(
arm_trajectory_action_name, FollowJointTrajectoryAction)
self._robotiq_pub = rospy.Publisher(robotiq_hand_command_topic,
SModelRobotOutput,
queue_size=1)
self._arm_joint_names = arm_joint_names
self._hand_joint_names = hand_joint_names
tolerance = JointTolerance()
self._path_tolerance = len(arm_joint_names) * [tolerance]
self._goal_tolerance = len(arm_joint_names) * [tolerance]
self.hand_interface = RobotiqHandInterface(robotiq_hand_command_topic,
robotiq_hand_state_topic,
robotiq_hand_joints_topic)
self._execution_running = False
self._current_arm_traj = None
self._current_hand_traj = None
self._current_traj = None
self._start_time = None
self._fake_feedback_timer = None
def __build_goal_message_ur5():
goal = FollowJointTrajectoryGoal()
goal.trajectory = JointTrajectory()
goal.trajectory.joint_names = ur5_joint_names
goal.goal_tolerance.append(JointTolerance('joint_tolerance', 0.1, 0.1, 0))
goal.goal_time_tolerance = rospy.Duration(5, 0)
return goal
def __init__(self, blackboard, _):
super(ExtendTorso, self).__init__(blackboard)
self.first_iteration = True
self.repeat = False
self.goal = FollowJointTrajectoryGoal()
torso_command = JointTrajectory()
torso_command.joint_names.append("torso_lift_joint")
jtp = JointTrajectoryPoint()
jtp.positions.append(0.35)
jtp.time_from_start = rospy.Duration.from_sec(2.0)
torso_command.points.append(jtp)
self.goal.trajectory = torso_command
jt = JointTolerance()
jt.name = 'torso_lift_joint'
jt.position = 0.01
self.goal.goal_tolerance.append(jt)
self.goal.goal_tolerance
def sendVelocityTrajectory(self, positions, vels, waitforresult=10.0):
# takes in a list of waypoint joint positions, positions, len(positions) = 8,
# where elements at indeces 0-6 inclusive correspond to position joint commands
# and element at index 7 in positions is the time argument for that waypoint
# and list of waypoint joint velocities, vels, len(vels) = 7
# where elements at indeces 0-6 inclusive correspond to velocity joint commands
bbottraj = JointTrajectory() # instantiate a JointTrajectory object
bbottraj.joint_names = self.joint_names # set joint names consistent with rostopic /bbot/joint_states
bbottraj.points = []
for i in range(len(vels)):
jtp = JointTrajectoryPoint()
# rospy.loginfo(str(vels[i][0:-1]))
# rospy.loginfo(str(len(vels[i][0:-1])))
jtp.positions = positions[i][0:-1]
jtp.velocities = vels[i]
jtp.time_from_start = rospy.Duration(positions[i][-1])
bbottraj.points.append(jtp)
rospy.loginfo(str(bbottraj.points))
self.goal.trajectory = bbottraj
# Tolerances applied to the joints as the trajectory is executed.
# if violated, error_code = PATH_TOLERANCE_VIOLATED = -4
self.goal.path_tolerance = []
# joint tolerances allowed for the final position
# if violated, error_code = GOAL_TOLERANCE_VIOLATED = -5
self.goal.goal_tolerance = []
# goal time tolerance
self.goal.goal_time_tolerance = rospy.Duration.from_sec(2)
# the joints are all currently set to a uniform tolerance
tol = JointTolerance()
tol.position = -1 # rad for revolute, m for prismatic
for j in bbottraj.joint_names:
self.goal.path_tolerance.append(tol)
self.goal.goal_tolerance.append(tol)
self.client.send_goal(
self.goal) # send the fully defined goal to the action server
self.client.wait_for_result(
rospy.Duration(waitforresult)) # wait for the goal to complete
return self.client.get_result()
def _fill_joint_trajectory_action_goal(self, joint_trajectory, path_tolerance, goal_tolerance, goal_time_tolerance):
n_joints = len(self.rox_robot.joint_names)
if not isinstance(path_tolerance, list):
path_tolerance1 = []
for n in self.rox_robot.joint_names:
path_tolerance1.append(JointTolerance(n, path_tolerance, 0, 0))
path_tolerance = path_tolerance1
if not isinstance(goal_tolerance, list):
goal_tolerance1 = []
for n in self.rox_robot.joint_names:
goal_tolerance1.append(JointTolerance(n, goal_tolerance, 0, 0))
goal_tolerance = goal_tolerance1
if not isinstance(goal_time_tolerance, rospy.Duration):
goal_time_tolerance = rospy.Duration(goal_time_tolerance)
return FollowJointTrajectoryGoal(joint_trajectory, path_tolerance, goal_tolerance, goal_time_tolerance)
def sendRandomGoal(self):
bbottraj = JointTrajectory() # instantiate a JointTrajectory object
bbottraj.joint_names = self.joint_names # set joint names consistent with rostopic /bbot/joint_states
bbottraj.points = []
maxcmd = 3.0
cmdlen = 2
for i in range(cmdlen):
jtp = JointTrajectoryPoint()
jtp.positions = [
random.random() * maxcmd,
random.random() * maxcmd,
random.random() * maxcmd,
random.random() * maxcmd,
random.random() * maxcmd,
random.random() * maxcmd,
random.random() * maxcmd
]
jtp.time_from_start = rospy.Duration(1.0 * i)
bbottraj.points.append(jtp)
self.goal.trajectory = bbottraj
# Tolerances applied to the joints as the trajectory is executed.
# if violated, error_code = PATH_TOLERANCE_VIOLATED = -4
self.goal.path_tolerance = []
# joint tolerances allowed for the final position
# if violated, error_code = GOAL_TOLERANCE_VIOLATED = -5
self.goal.goal_tolerance = []
# the joints are all currently set to a uniform tolerance
tol = JointTolerance()
tol.position = 1.0 # rad for revolute, m for prismatic
for j in bbottraj.joint_names:
self.goal.path_tolerance.append(tol)
self.goal.goal_tolerance.append(tol)
self.client.send_goal(
self.goal) # send the fully defined goal to the action server
self.client.wait_for_result(
rospy.Duration.from_sec(cmdlen)) # wait for the goal to complete
return self.client.get_result()
def createHeadGoal(j1, j2):
"""Creates a FollowJointTrajectoryGoal with the values specified in j1 and j2 for the joint positions
@arg j1 float value for head_1_joint
@arg j2 float value for head_2_joint
@returns FollowJointTrajectoryGoal with the specified goal"""
fjtg = FollowJointTrajectoryGoal()
fjtg.trajectory.joint_names.append('head_1_joint')
fjtg.trajectory.joint_names.append('head_2_joint')
point = JointTrajectoryPoint()
point.positions.append(j1)
point.positions.append(j2)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.time_from_start = rospy.Duration(4.0)
for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
fjtg.goal_tolerance.append(goal_tol)
fjtg.goal_time_tolerance = rospy.Duration(3)
fjtg.trajectory.points.append(point)
fjtg.trajectory.header.stamp = rospy.Time.now()
return fjtg
def createHandGoal(side, j1, j2, j3):
"""Creates a FollowJointTrajectoryGoal with the values specified in j1, j2 and j3 for the joint positions
with the hand specified in side
@arg side string 'right' or 'left'
@arg j1 float value for joint 'hand_'+side+'_thumb_joint'
@arg j2 float value for joint 'hand_'+side+'_middle_joint'
@arg j3 float value for joint 'hand_'+side+'_index_joint'
@return FollowJointTrajectoryGoal with the specified goal"""
fjtg = FollowJointTrajectoryGoal()
fjtg.trajectory.joint_names.append('hand_' + side + '_thumb_joint')
fjtg.trajectory.joint_names.append('hand_' + side + '_middle_joint')
fjtg.trajectory.joint_names.append('hand_' + side + '_index_joint')
point = JointTrajectoryPoint()
point.positions.append(j1)
point.positions.append(j2)
point.positions.append(j3)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.time_from_start = rospy.Duration(4.0)
fjtg.trajectory.points.append(point)
for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
fjtg.goal_tolerance.append(goal_tol)
fjtg.goal_time_tolerance = rospy.Duration(3)
fjtg.trajectory.header.stamp = rospy.Time.now()
return fjtg
def execute(self, userdata):
rospy.loginfo('In create_move_group_joints_goal')
_move_joint_goal = FollowJointTrajectoryGoal()
_move_joint_goal.trajectory.joint_names = userdata.move_joint_list
jointTrajectoryPoint = JointTrajectoryPoint()
jointTrajectoryPoint.positions = userdata.move_joint_poses
for x in range(0, len(userdata.move_joint_poses)):
jointTrajectoryPoint.velocities.append(0.0)
jointTrajectoryPoint.time_from_start = rospy.Duration(2.0)
_move_joint_goal.trajectory.points.append(jointTrajectoryPoint)
for joint in _move_joint_goal.trajectory.joint_names:
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
_move_joint_goal.goal_tolerance.append(goal_tol)
_move_joint_goal.goal_time_tolerance = rospy.Duration(2.0)
_move_joint_goal.trajectory.header.stamp = rospy.Time.now()
rospy.loginfo('Move_Joint_GOAL: ' + str(_move_joint_goal))
userdata.move_joint_goal = _move_joint_goal
return 'succeeded'
def make_joint_tolerance(pos, name):
j = JointTolerance()
j.position = pos
j.velocity = 1
j.acceleration = 100 # this is high because if we bump the arm, the controller will abort unnecessarily
j.name = name
return j
def execute(self,traj,speed=DEFAULT_SPEED,joint_tolerance=DEFAULT_JOINT_TOLERANCE,speed_tolerance=DEFAULT_SPEED_TOLERANCE,timeout=DEFAULT_TIMEOUT,path_tolerance=0):
traj = deepcopy(traj)
self.applyTrajectorySpeed(traj,speed)
rospy.logdebug("Performing trajectory on %s at speed %f tolerance %f speed_tolerance %f timeout %f"%(self.side,speed,joint_tolerance,speed_tolerance,timeout))
joint_names=traj.joint_trajectory.joint_names
self.waitForStopped()
with self.moving_lock:
if self.is_moving!=0:
rospy.logerr("ERROR: trajectory already running")
return False
self.is_moving=1
goal=FollowJointTrajectoryActionGoal()
self.current_goal_id+=1
goal.goal_id.id=self.side+"_"+str(self.current_goal_id)
goal.goal.trajectory=traj.joint_trajectory
if path_tolerance<=0: path_tolerance=DEFAULT_PATH_TOLERANCE*speed
for name in joint_names:
t=JointTolerance()
t.name=name
t.position=joint_tolerance
t.velocity=speed_tolerance
t.acceleration=0
goal.goal.goal_tolerance.append(t)
pt=JointTolerance()
pt.name=name
if name.find("w2") is False:
pt.position=path_tolerance
else:
pt.position=0.0
goal.goal.path_tolerance.append(pt)
goal.goal.goal_time_tolerance=rospy.Duration(timeout)
# Send goal and wait for trajectory finished
self.pub_traj_goal.publish(goal)
self.waitForStopped()
return not self.result_error
def create_position_goal(self, absolute, position_setpoint, velocity):
current_position = self.message.position
# set up variables
trajectory = JointTrajectory()
point = JointTrajectoryPoint()
# Final position
if (absolute == ActuatorsState.ABS):
# absolute move
point.positions = [position_setpoint]
else:
# Relative move
point.positions = [current_position + position_setpoint]
point.velocities = [0] # cubic spline in position, smooth moves
# point.accelerations = [0] # cubic spline in position, smooth moves
trajectory.points.append(point)
# How long to get there
if (velocity != 0):
time_for_move = abs(current_position -
position_setpoint) / velocity
else:
time_for_move = 0 # porbably should handle different
if (time_for_move <= 0.1):
time_for_move = 0.1 # probably asserting joint.
trajectory.points[0].time_from_start = rospy.Duration(time_for_move)
# specify which joint to move
trajectory.joint_names.append(self.message.name)
follow_trajectory_goal = FollowJointTrajectoryGoal()
follow_trajectory_goal.trajectory = trajectory
# TOLERANCES FOR MOVE
# For some reason the defaults are not being loaded from control.yaml
tolerance = JointTolerance()
tolerance.name = self.message.name # joint name
tolerance.velocity = -1 # no fault for velocity
tolerance.acceleration = -1 # no fault for acceleration
# Tracking fault
tolerance.position = -1
follow_trajectory_goal.path_tolerance.append(tolerance)
# End position fault
tolerance.position = -1 # p.constraints.goal_pos
follow_trajectory_goal.goal_tolerance.append(tolerance)
# Time to reach goal precision
follow_trajectory_goal.goal_time_tolerance = rospy.Duration(10.0)
return follow_trajectory_goal
def test_joint_passthrough(self):
#### Power cycle the robot in order to make sure it is running correctly####
self.assertTrue(self.set_robot_to_mode(RobotMode.POWER_OFF))
rospy.sleep(0.5)
self.assertTrue(self.set_robot_to_mode(RobotMode.RUNNING))
rospy.sleep(0.5)
self.send_program_srv.call()
rospy.sleep(0.5) # TODO properly wait until the controller is running
self.switch_on_controller("forward_joint_traj_controller")
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
"elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
position_list = [[0, -1.57, -1.57, 0, 0, 0]]
position_list.append([0.2, -1.57, -1.57, 0, 0, 0])
position_list.append([-0.5, -1.57, -1.2, 0, 0, 0])
duration_list = [3.0, 7.0, 10.0]
for i, position in enumerate(position_list):
point = JointTrajectoryPoint()
point.positions = position
point.time_from_start = rospy.Duration(duration_list[i])
goal.trajectory.points.append(point)
for i, joint_name in enumerate(goal.trajectory.joint_names):
goal.goal_tolerance.append(
JointTolerance(joint_name, 0.2, 0.2, 0.2))
goal.goal_time_tolerance = rospy.Duration(0.6)
self.joint_passthrough_trajectory_client.send_goal(goal)
self.joint_passthrough_trajectory_client.wait_for_result()
self.assertEqual(
self.joint_passthrough_trajectory_client.get_result().error_code,
FollowJointTrajectoryResult.SUCCESSFUL)
rospy.loginfo("Received result SUCCESSFUL")
p1.velocities = [0, 0, 0, 0, 0, 0]
p1.time_from_start = rospy.Duration.from_sec(3.0)
p2 = JointTrajectoryPoint()
p2.positions = [0.5, 0.5, 0, 0, 0, 0]
p2.velocities = [0, 0, 0, 0, 0, 0]
p2.time_from_start = rospy.Duration.from_sec(5.0)
p3 = JointTrajectoryPoint()
p3.positions = [0, 0, 0, 0, 0, 0]
p3.velocities = [0, 0, 0, 0, 0, 0]
p3.time_from_start = rospy.Duration.from_sec(7.0)
goal.trajectory.points = [p1, p2, p3]
pt1 = JointTolerance()
pt1.name = 'joint_1'
pt1.position = 10.1
pt1.velocity = 10.1
pt1.acceleration = 10.1
pt2 = JointTolerance()
pt2.name = 'joint_2'
pt2.position = 10.1
pt2.velocity = 10.1
pt2.acceleration = 10.1
pt3 = JointTolerance()
pt3.name = 'joint_3'
pt3.position = 10.1
pt3.velocity = 10.1
pt3.acceleration = 10.1
pt4 = JointTolerance()
def on_enter(self, userdata):
# Clear data
userdata.joint_tolerances = None
self.return_code = None
try:
num_names = len(userdata.joint_names)
num_posn = len(self.position_constraints)
num_vel = len(self.velocity_constraints)
num_acc = len(self.acceleration_constraints)
if (num_posn > 1 and num_posn != num_names):
Logger.logwarn(
'Mis-match in joint names and position constraints -%d vs. %d'
% (num_names, num_posn))
self.return_code = 'param_error'
if (num_vel > 1 and num_vel != num_names):
Logger.logwarn(
'Mis-match in joint names and position constraints - %d vs. %d'
% (num_names, num_vel))
self.return_code = 'param_error'
if (num_acc > 1 and num_acc != num_names):
Logger.logwarn(
'Mis-match in joint names and position constraints - %d vs. %d'
% (num_names, num_acc))
self.return_code = 'param_error'
if self.return_code is not None:
return self.return_code
tolerances = []
for ndx, name in enumerate(userdata.joint_names):
jt = JointTolerance()
jt.name = name
if (num_posn > 1):
jt.position = self.position_constraints[ndx]
else:
jt.position = self.position_constraints[0]
if (num_vel > 1):
jt.velocity = self.velocity_constraints[ndx]
else:
jt.velocity = self.velocity_constraints[0]
if (num_acc > 1):
jt.acceleration = self.acceleration_constraints[ndx]
else:
jt.acceleration = self.acceleration_constraints[0]
tolerances.append(jt)
# Successfully set the joint tolerances and added to user data
userdata.joint_tolerances = tolerances
self.return_code = 'configured'
except Exception as e:
Logger.logwarn('Unable to configure joint tolerances for %s \n%s' %
(self.name, str(e)))
self.return_code = 'param_error'
return
def wipe(self, start, finish):
dist = int(round(
200 *
self.calc_dist(start, finish))) # 1 point per cm of separation
print "Steps: %s" % dist
x_step = finish.pose.position.x - start.pose.position.x
y_step = finish.pose.position.y - start.pose.position.y
z_step = finish.pose.position.z - start.pose.position.z
#print "Increments: %s,%s,%s" %(x_step, y_step, z_step)
qs = [
start.pose.orientation.x, start.pose.orientation.y,
start.pose.orientation.z, start.pose.orientation.w
]
qf = [
finish.pose.orientation.x, finish.pose.orientation.y,
finish.pose.orientation.z, finish.pose.orientation.w
]
steps = []
#print "Start: %s" %start
#print "Finish: %s" %finish
for i in range(dist):
frac = float(i) / float(dist)
steps.append(PoseStamped())
steps[i].header.stamp = rospy.Time.now()
steps[i].header.frame_id = start.header.frame_id
steps[i].pose.position.x = start.pose.position.x + x_step * frac
steps[i].pose.position.y = start.pose.position.y + y_step * frac
steps[i].pose.position.z = start.pose.position.z + z_step * frac
new_q = transformations.quaternion_slerp(qs, qf, frac)
steps[i].pose.orientation.x = new_q[0]
steps[i].pose.orientation.y = new_q[1]
steps[i].pose.orientation.z = new_q[2]
steps[i].pose.orientation.w = new_q[3]
steps.append(finish)
#print "Steps:"
#print steps
#raw_input("Press Enter to continue")
rospy.loginfo("Planning straight-line path, please wait")
self.wt_log_out.publish(
data="Planning straight-line path, please wait")
rospy.loginfo("Initiating wipe action")
self.blind_move(finish)
self.r_arm_traj_client.wait_for_result(rospy.Duration(20))
rospy.loginfo("At beginning of path")
pts = []
for i, p in enumerate(steps):
frac = float(i) / float(len(steps))
request = self.form_ik_request(p)
if not i == 0:
request.ik_request.ik_seed_state.joint_state.position = seed
ik_goal = self.ik_pose_proxy(request)
pts.append(ik_goal.solution.joint_state.position)
seed = pts[i]
points = []
for i in range(len(pts) - 1):
angs1 = pts[i]
angs2 = pts[i + 1]
increm = np.subtract(angs2, angs1)
for j in range(10):
points.append(np.add(angs1, np.multiply(0.1 * j, increm)))
#points = np.unwrap(points,1)
p1 = points
traj = JointTrajectory()
traj.header.frame_id = steps[0].header.frame_id
traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
times = []
for i in range(len(points)):
frac = float(i) / float(len(points))
traj.points.append(JointTrajectoryPoint())
traj.points[i].positions = points[i]
traj.points[i].velocities = [0] * 7
times.append(rospy.Duration(frac * dist * 0.2))
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = traj
tolerance = JointTolerance()
tolerance.position = 0.05
tolerance.velocity = 0.1
traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_time_tolerance = rospy.Duration(3)
#print "Steps: %s" %steps
count = 0
while count < 6:
traj_goal.trajectory.points.reverse()
for i in range(len(times)):
traj_goal.trajectory.points[i].time_from_start = times[i]
if count == 0:
print traj_goal.trajectory.points
raw_input("Review Trajectory Goal")
traj_goal.trajectory.header.stamp = rospy.Time.now()
self.r_arm_follow_traj_client.send_goal(traj_goal)
self.r_arm_follow_traj_client.wait_for_result(rospy.Duration(20))
rospy.sleep(0.5)
count += 1
#traj_goal = JointTrajectoryGoal()
#traj_goal.trajectory = traj
#print "Steps: %s" %steps
#count = 0
#
#while count < 6:
#traj_goal.trajectory.points.reverse()
#for i in range(len(times)):
#traj_goal.trajectory.points[i].time_from_start = times[i]
#print traj_goal
#raw_input("Review Trajectory Goal")
##print "Traj goal start:"
##print traj_goal.trajectory.points[0].positions
##print "Traj goal end:"
##print traj_goal.trajectory.points[-1].positions
#traj_goal.trajectory.header.stamp = rospy.Time.now()
#self.r_arm_traj_client.send_goal(traj_goal)
#self.r_arm_traj_client.wait_for_result(rospy.Duration(20))
#rospy.sleep(0.5)
#count += 1
rospy.loginfo("Done Wiping")
self.wt_log_out.publish(data="Done Wiping")
self.linear_move(Float32(-0.15))
def wipe(self, start, finish):
dist = int(round(200*self.calc_dist(start, finish))) # 1 point per cm of separation
print "Steps: %s" %dist
x_step = finish.pose.position.x - start.pose.position.x
y_step = finish.pose.position.y - start.pose.position.y
z_step = finish.pose.position.z - start.pose.position.z
#print "Increments: %s,%s,%s" %(x_step, y_step, z_step)
qs = [start.pose.orientation.x, start.pose.orientation.y, start.pose.orientation.z, start.pose.orientation.w]
qf = [finish.pose.orientation.x, finish.pose.orientation.y, finish.pose.orientation.z, finish.pose.orientation.w]
steps = []
#print "Start: %s" %start
#print "Finish: %s" %finish
for i in range(dist):
frac = float(i)/float(dist)
steps.append(PoseStamped())
steps[i].header.stamp = rospy.Time.now()
steps[i].header.frame_id = start.header.frame_id
steps[i].pose.position.x = start.pose.position.x + x_step*frac
steps[i].pose.position.y = start.pose.position.y + y_step*frac
steps[i].pose.position.z = start.pose.position.z + z_step*frac
new_q = transformations.quaternion_slerp(qs,qf,frac)
steps[i].pose.orientation.x = new_q[0]
steps[i].pose.orientation.y = new_q[1]
steps[i].pose.orientation.z = new_q[2]
steps[i].pose.orientation.w = new_q[3]
steps.append(finish)
#print "Steps:"
#print steps
#raw_input("Press Enter to continue")
rospy.loginfo("Planning straight-line path, please wait")
self.wt_log_out.publish(data="Planning straight-line path, please wait")
rospy.loginfo("Initiating wipe action")
self.blind_move(finish)
self.r_arm_traj_client.wait_for_result(rospy.Duration(20))
rospy.loginfo("At beginning of path")
pts = []
for i, p in enumerate(steps):
frac = float(i)/float(len(steps))
request = self.form_ik_request(p)
if not i == 0:
request.ik_request.ik_seed_state.joint_state.position = seed
ik_goal = self.ik_pose_proxy(request)
pts.append(ik_goal.solution.joint_state.position)
seed = pts[i]
points = []
for i in range(len(pts)-1):
angs1 = pts[i]
angs2 = pts[i+1]
increm = np.subtract(angs2, angs1)
for j in range(10):
points.append(np.add(angs1, np.multiply(0.1*j, increm)))
#points = np.unwrap(points,1)
p1= points
traj = JointTrajectory()
traj.header.frame_id = steps[0].header.frame_id
traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
times = []
for i in range(len(points)):
frac = float(i)/float(len(points))
traj.points.append(JointTrajectoryPoint())
traj.points[i].positions = points[i]
traj.points[i].velocities = [0]*7
times.append(rospy.Duration(frac*dist*0.2))
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = traj
tolerance = JointTolerance()
tolerance.position = 0.05
tolerance.velocity = 0.1
traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_time_tolerance = rospy.Duration(3)
#print "Steps: %s" %steps
count = 0
while count < 6:
traj_goal.trajectory.points.reverse()
for i in range(len(times)):
traj_goal.trajectory.points[i].time_from_start = times[i]
if count == 0:
print traj_goal.trajectory.points
raw_input("Review Trajectory Goal")
traj_goal.trajectory.header.stamp = rospy.Time.now()
self.r_arm_follow_traj_client.send_goal(traj_goal)
self.r_arm_follow_traj_client.wait_for_result(rospy.Duration(20))
rospy.sleep(0.5)
count += 1
#traj_goal = JointTrajectoryGoal()
#traj_goal.trajectory = traj
#print "Steps: %s" %steps
#count = 0
#
#while count < 6:
#traj_goal.trajectory.points.reverse()
#for i in range(len(times)):
#traj_goal.trajectory.points[i].time_from_start = times[i]
#print traj_goal
#raw_input("Review Trajectory Goal")
##print "Traj goal start:"
##print traj_goal.trajectory.points[0].positions
##print "Traj goal end:"
##print traj_goal.trajectory.points[-1].positions
#traj_goal.trajectory.header.stamp = rospy.Time.now()
#self.r_arm_traj_client.send_goal(traj_goal)
#self.r_arm_traj_client.wait_for_result(rospy.Duration(20))
#rospy.sleep(0.5)
#count += 1
rospy.loginfo("Done Wiping")
self.wt_log_out.publish(data="Done Wiping")
self.linear_move(Float32(-0.15))
