def move_head(self, pan , tilt):
# Which joints define the head?
head_joints = ['head_pan_joint', 'head_tilt_mod_joint']
# Create a single-point head trajectory with the head_goal as the end-point
head_trajectory = JointTrajectory()
head_trajectory.joint_names = head_joints
head_trajectory.points.append(JointTrajectoryPoint())
head_trajectory.points[0].positions = pan , tilt
head_trajectory.points[0].velocities = [0.0 for i in head_joints]
head_trajectory.points[0].accelerations = [0.0 for i in head_joints]
head_trajectory.points[0].time_from_start = rospy.Duration(3.0)
# Send the trajectory to the head action server
rospy.loginfo('Moving the head to goal position...')
head_goal = FollowJointTrajectoryGoal()
head_goal.trajectory = head_trajectory
head_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal
self.head_client.send_goal(head_goal)
# Wait for up to 5 seconds for the motion to complete
self.head_client.wait_for_result(rospy.Duration(2.0))
rospy.loginfo('...done')
def __init__(self):
rospy.init_node('arm_simple_trajectory')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Which joints define the arm?
arm_joints = ['joint_lift',
'joint_waist',
'joint_big_arm',
'joint_forearm',
'joint_wrist_pitching',
'joint_wrist_rotation']
if reset:
# Set the arm back to the resting position
arm_goal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
else:
# Set a goal configuration for the arm
arm_goal = [1.0, 1.0, 1.0, 1.0, 0.0, 0.0]
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for xm arm trajectory controller...')
arm_client = actionlib.SimpleActionClient('xm_arm_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo('...connected.')
rospy.sleep(1)
# Create a single-point arm trajectory with the arm_goal as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(3)
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
rospy.sleep(1)
# Create an empty trajectory goal
arm_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
arm_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
arm_goal.goal_time_tolerance = rospy.Duration(0)
# Send the goal to the action server
arm_client.send_goal(arm_goal)
rospy.loginfo('...done')
rospy.sleep(1)
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 move_to(self, result_trajectory, duration=5.0):
trajectory = result_trajectory
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
self.client.send_goal(follow_goal)
self.client.wait_for_result()
def send_arm_goal(self, arm_goal):
arm_joint_names = ['joint_lift',
'joint_waist',
'joint_big_arm',
'joint_forearm',
'joint_wrist_pitching',
'joint_wrist_rotation']
rospy.loginfo("Waiting for follow_joint_trajectory server")
self.arm_client.wait_for_server()
rospy.loginfo("Connected to follow_joint_trajectory server")
rospy.sleep(1)
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joint_names
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].time_from_start = rospy.Duration(10)
rospy.loginfo("Preparing for moving the arm to goal position!")
rospy.sleep(1)
arm_goal_pos = FollowJointTrajectoryGoal()
arm_goal_pos.trajectory = arm_trajectory
arm_goal_pos.goal_time_tolerance = rospy.Duration(0)
self.arm_client.send_goal(arm_goal_pos)
rospy.loginfo("Send goal to the trajectory server successfully!")
self.arm_client.wait_for_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 execute_trajectory(self, trajectory, duration=5.0):
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
self.client.send_goal(follow_goal)
#rospy.sleep(1.5)
#self.client.cancel_goal()
self.client.wait_for_result()
self.finish_time = rospy.Time.now()
self.believed_state = trajectory.points[-1].positions
def __init__(self):
rospy.init_node('trajectory_demo')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Which joints define the arm?
arm_joints = ['link_1_joint',
'servo_2_joint',
'link_3_joint',
'link_4_joint',
'link_5_joint']
if reset:
# Set the arm back to the resting position
arm_goal = [0, 0, 0, 0, 0]
else:
# Set a goal configuration for the arm
arm_goal = [-0.525487, 0.904972, 0.480017, -1.331166, 0.731413]
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
arm_client = actionlib.SimpleActionClient('right_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo('...connected.')
# Create a single-point arm trajectory with the arm_goal as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
# Create an empty trajectory goal
arm_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
arm_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal to the action server
arm_client.send_goal(arm_goal)
rospy.loginfo('...done')
def move_to(self, positions, duration=5.0):
print "============= Moving"
trajectory = JointTrajectory()
trajectory.joint_names = self.joint_names
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = positions
trajectory.points[0].velocities = [0.0 for _ in positions]
trajectory.points[0].accelerations = [0.0 for _ in positions]
trajectory.points[0].time_from_start = rospy.Duration(duration)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
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 btnPlay(self):
if not self.ui.chkAllMotors.isChecked():
print("all motors should have torque")
return
if self.ui.spinSetFrame.value()>self.ui.spinPlayTo.value() or self.ui.spinSetFrame.value()<1 or self.ui.spinPlayTo.value()>self.totalFrames:
print("\nwrong range in play\n")
return
trajectories=[]
for cat in self.motorTrajectoryTopics:
jointTraj=JointTrajectory()
jts_names=[]
for name in self.names:
#if self.motorNamesAndCat[name]==cat and name!='l_hip_yaw':
if self.motorNamesAndCat[name]==cat:
jointTraj.joint_names.append(name+"_joint")
jts_names.append(name)
fcount=self.ui.spinPlayTo.value()-self.ui.spinSetFrame.value()+1
totalDelay=0.0
for n in range(0,fcount):
pos_arr=JointTrajectoryPoint()
readLoc=n+self.ui.spinSetFrame.value()
delaySecs=float(self.TimeDelayFromPrevious[readLoc-1])/1000.0
totalDelay=totalDelay+delaySecs
pos_arr.time_from_start=rospy.Duration().from_sec(totalDelay)
for name in jts_names:
pos_arr.positions.append(self.TrajectoryInfo[name][readLoc-1])
print("\n "+name+": "+str(self.TrajectoryInfo[name][readLoc-1]))
velocity=1.0#2*pi rad/sec
if n!=0:
velocity=float(self.TrajectoryInfo[name][readLoc-1]-self.TrajectoryInfo[name][readLoc-2])/delaySecs
##pos_arr.velocities.append(velocity)
jointTraj.points.append(pos_arr)
jointTraj.header.stamp=rospy.Time.now()
trajectories.append(jointTraj)
trajClients=[]
mm=0
for cat in self.motorTrajectoryTopics:
trajClients.append(actionlib.SimpleActionClient(self.motorTrajectoryTopics[cat],FollowJointTrajectoryAction))
trajClients[mm].wait_for_server()
mm=mm+1
#trajectories[nn] .. how to send this goal
nn=0
for traj in trajClients:
goal=FollowJointTrajectoryGoal()
#goal.trajectory.points=trajectories[nn].points
goal.trajectory=trajectories[nn]
traj.send_goal(goal)
nn=nn+1
print ("\nDone Sending Play Info...\n")
def move_to_start(self):
self.moving_mode = True
# Look down
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = '/base_link'
goal.target.point.x = 1.5
goal.target.point.y = 0.0
goal.target.point.z = -0.2
goal.min_duration = rospy.Duration(0.5)
if 0: logger.info('Point head to %s...', goal);
self.head_point_client.send_goal(goal)
if 0: logger.info('Point head sent')
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
joints = [
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
]
pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0]
result = self.arm_move_group.moveToJointPosition(joints, pose, plan_only=True)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
if 0: logger.info('Got trajectory %s', result.planned_trajectory)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = result.planned_trajectory.joint_trajectory
logger.info('sending trajectory to arm...')
self.arm_trajectory_client.send_goal(follow_goal)
result = self.arm_trajectory_client.wait_for_result()
logger.info('arm followed trajectory %s', result)
else:
logger.warn('moveToJointPosition returned %s', result)
return
result = self.head_point_client.wait_for_result()
logger.info('head followed trajectory %s', result)
logger.info('sending empty arm goal')
empty_goal = FollowJointTrajectoryGoal()
self.arm_trajectory_client.send_goal(empty_goal)
logger.info('Point head done')
self.moving_mode = False
def execute(self, trajectory, test=None, epsilon=0.1):
"""
Safely executes a trajectory in joint space on the robot or simulate through RViz and its Moveit plugin (File moveit.rviz must be loaded into RViz)
This method is BLOCKING until the command succeeds or failure occurs i.e. the user interacted with the cuff or collision has been detected
Non-blocking needs should deal with the JointTrajectory action server
:param trajectory: either a RobotTrajectory or a JointTrajectory
:param test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param epsilon: distance threshold on the first point. If distance with first point of the trajectory is greater than espilon execute a controlled trajectory to the first point. Put float(inf) value to bypass this functionality
:return: True if execution ended successfully, False otherwise
"""
def distance_to_first_point(point):
joint_pos = np.array(point.positions)
return np.linalg.norm(current_array - joint_pos)
self.display(trajectory)
with self._stop_lock:
self._stop_reason = ''
if isinstance(trajectory, RobotTrajectory):
trajectory = trajectory.joint_trajectory
elif not isinstance(trajectory, JointTrajectory):
raise TypeError("Execute only accept RobotTrajectory or JointTrajectory")
ftg = FollowJointTrajectoryGoal()
ftg.trajectory = trajectory
# check if it is necessary to move to the first point
current = self.get_current_state()
current_array = np.array([current.joint_state.position[current.joint_state.name.index(joint)] for joint in trajectory.joint_names])
if distance_to_first_point(trajectory.points[0]) > epsilon:
# convert first point to robot state
rs = RobotState()
rs.joint_state.name = trajectory.joint_names
rs.joint_state.position = trajectory.points[0].positions
# move to the first point
self.move_to_controlled(rs)
# execute the input trajectory
self.client.send_goal(ftg)
# Blocking part, wait for the callback or a collision or a user manipulation to stop the trajectory
while self.client.simple_state != SimpleGoalState.DONE:
if callable(test) and test():
self.client.cancel_goal()
return True
if self._stop_reason!='':
self.client.cancel_goal()
return False
self._rate.sleep()
return True
def __init__(self):
rospy.init_node("gripper_trajectory_demo")
# Set to True to move back to the starting configurations
reset = rospy.get_param("~reset", False)
# Set the duration of the trajectory in seconds
duration = rospy.get_param("~duration", 2.0)
# Which joints define the head?
gripper_joints = ["right_gripper_finger_joint"]
# Set a goal configuration for the head
if reset:
gripper_goal = [0]
else:
gripper_goal = [0.25]
# Connect to the head trajectory action server
rospy.loginfo("Waiting for gripper trajectory controller...")
gripper_client = actionlib.SimpleActionClient(
"right_gripper_controller/follow_joint_trajectory", FollowJointTrajectoryAction
)
gripper_client.wait_for_server()
rospy.loginfo("...connected.")
# Create a head trajectory with a single end point using the gripper_goal positions
trajectory = JointTrajectory()
trajectory.joint_names = gripper_joints
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = gripper_goal
trajectory.points[0].velocities = [0.0 for i in gripper_joints]
trajectory.points[0].accelerations = [0.0 for i in gripper_joints]
trajectory.points[0].time_from_start = rospy.Duration(duration)
# Send the trajectory to the head action server
rospy.loginfo("Moving the gripper to goal position...")
goal = FollowJointTrajectoryGoal()
goal.trajectory = trajectory
goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal
gripper_client.send_goal(goal)
# Wait for up to 5 seconds for the motion to complete
gripper_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo("...done")
def get_hand_goal(goal_positions):
grasp_msg = FollowJointTrajectoryGoal()
point1 = JointTrajectoryPoint(
positions=goal_positions,
velocities=[0.0, 0.0, 0.0],
time_from_start = rospy.Duration(1))
grasp_msg.trajectory = JointTrajectory(
joint_names = ['hand_right_thumb_joint', 'hand_right_index_1_joint', 'hand_right_middle_1_joint'],
points = [point1])
return grasp_msg
def on_enter(self, userdata):
self._failed = False
# create goal
goal = FollowJointTrajectoryGoal()
goal.trajectory = userdata.joint_trajectory
goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
# execute the motion
try:
self._client.send_goal(self._action_topic, goal)
except Exception as e:
Logger.logwarn('Was unable to execute joint trajectory:\n%s' % str(e))
self._failed = True
def __init__(self):
rospy.init_node('head_trajectory_demo')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Set the duration of the trajectory in seconds
duration = rospy.get_param('~duration', 3.0)
# Which joints define the head?
head_joints = ['head_pan_joint', 'head_tilt_joint']
# Set a goal configuration for the head
if reset:
head_goal = [0, 0]
else:
head_goal = [-1.3, -0.3]
# Connect to the head trajectory action server
rospy.loginfo('Waiting for head trajectory controller...')
head_client = actionlib.SimpleActionClient('head_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
head_client.wait_for_server()
rospy.loginfo('...connected.')
# Create a head trajectory with a single end point using the head_goal positions
trajectory = JointTrajectory()
trajectory.joint_names = head_joints
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = head_goal
trajectory.points[0].velocities = [0.0 for i in head_joints]
trajectory.points[0].accelerations = [0.0 for i in head_joints]
trajectory.points[0].time_from_start = rospy.Duration(duration)
# Send the trajectory to the head action server
rospy.loginfo('Moving the head to goal position...')
goal = FollowJointTrajectoryGoal()
goal.trajectory = trajectory
goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal
head_client.send_goal(goal)
# Wait for up to 5 seconds for the motion to complete
head_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo('...done')
def execute_traj_moveit(self, waypoints):
# Cycle through waypoints
for point in waypoints:
plannedTraj = self.arm_planner.plan_jointTargetInput(point)
if plannedTraj == None or len(plannedTraj.joint_trajectory.points) < 1:
print "Error: no plan found"
return -1
else:
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = plannedTraj.joint_trajectory
self.smooth_joint_trajectory_client.send_goal(traj_goal)
self.smooth_joint_trajectory_client.wait_for_result()
self.smooth_joint_trajectory_client.get_result()
return 1
def move_to(self, positions, duration=5.0):
if len(self.joint_names) != len(positions):
print("Invalid trajectory position")
return False
trajectory = JointTrajectory()
trajectory.joint_names = self.joint_names
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = positions
trajectory.points[0].velocities = [0.0 for _ in positions]
trajectory.points[0].accelerations = [0.0 for _ in positions]
trajectory.points[0].time_from_start = rospy.Duration(duration)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
self.client.send_goal(follow_goal)
self.client.wait_for_result()
def move_joint(angles):
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = ['arm_base_joint', 'shoulder_joint','bottom_wrist_joint' ,'elbow_joint', 'top_wrist_joint']
point = JointTrajectoryPoint()
point.positions = angles
point.time_from_start = rospy.Duration(3)
goal.trajectory.points.append(point)
client.send_goal_and_wait(goal)
def move_to(self, positions, duration=5.0):
if len(self.joint_names) != len(positions):
print("Invalid position goal: got %d positions,
can only handle one." %len(positions))
return False
trajectory = JointTrajectory()
trajectory.joint_names = self.joint_names
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = positions
trajectory.points[0].velocities = [0.0 for _ in positions]
trajectory.points[0].accelerations = [0.0 for _ in positions]
trajectory.points[0].time_from_start = rospy.Duration(duration)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
self.client.send_goal(follow_goal)
self.client.wait_for_result()
def createTraj(self, point): self.joints_position = [] self.names =["joint1", "joint2", "joint3", "joint4", "gripper"] dur = rospy.Duration(1) pointTraj = JointTrajectoryPoint(positions=point, velocities=[0.5]*self.Joints, time_from_start=dur) self.joints_position.append(pointTraj) self.TrajPoint = JointTrajectory(joint_names=self.names, points=self.joints_position) self.TrajGoal = FollowJointTrajectoryGoal(trajectory=self.TrajPoint, goal_time_tolerance=rospy.Duration(3))
def _create_goal(self, joints_position, duration=2):
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = self._joint_names
goal.trajectory.points = [JointTrajectoryPoint()]
goal.trajectory.points[0].positions = joints_position
goal.trajectory.points[0].velocities = [0.0] * len(self._joint_names)
goal.trajectory.points[0].time_from_start = rospy.Duration(duration)
return goal
def move_joint(self, angles):
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = ['shoulder_1_joint', 'shoulder_2_joint']
point = JointTrajectoryPoint()
point.positions = angles
point.time_from_start = rospy.Duration(3)
goal.trajectory.points.append(point)
self.jta.send_goal_and_wait(goal)
def move_to_point(self, point, joint_names=[''], interval=0.8):
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = self.joint_names
point.time_from_start = rospy.Duration(interval)
goal.trajectory.points.append(point)
rospy.loginfo('Sending new goal for {}'.format(self.name))
self.jta_client.send_goal(goal)
self.clear()
def move_to_initial(self, positions, duration):
assert(len(self.joint_names) == len(positions))
trajectory = JointTrajectory()
trajectory.joint_names = self.joint_names
p = JointTrajectoryPoint()
p.positions = positions
p.time_from_start = rospy.Duration(duration)
trajectory.points.append(p)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
self.client.send_goal(follow_goal)
self.client.wait_for_result()
def move_arm_joints(client, arm_joint_positions, duration=5.0):
trajectory = JointTrajectory()
trajectory.joint_names = ARM_JOINT_NAMES
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = arm_joint_positions
trajectory.points[0].velocities = [0.0] * len(arm_joint_positions)
trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions)
trajectory.points[0].time_from_start = rospy.Duration(duration)
arm_goal = FollowJointTrajectoryGoal()
arm_goal.trajectory = trajectory
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
print("Moving amr to joints: ", arm_joint_positions)
client.send_goal(arm_goal)
client.wait_for_result(rospy.Duration(duration + 1.0))
rospy.loginfo("...done")
def move_head_joints(client, head_joint_positions):
trajectory = JointTrajectory()
trajectory.joint_names = HEAD_JOINT_NAMES
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = head_joint_positions
trajectory.points[0].velocities = [0.0] * len(head_joint_positions)
trajectory.points[0].accelerations = [0.0] * len(head_joint_positions)
trajectory.points[0].time_from_start = rospy.Duration(1.0)
head_goal = FollowJointTrajectoryGoal()
head_goal.trajectory = trajectory
head_goal.goal_time_tolerance = rospy.Duration(0.0)
print("Moving head to joints: ", head_joint_positions)
client.send_goal(head_goal)
client.wait_for_result(rospy.Duration(1.0))
rospy.loginfo("...done")
def __init__(self):
rospy.init_node('trajectory_demo')
# Which joints define the arm?
arm_joints = ['x_joint']
# Set the arm back to the resting position
arm_goal = [2]
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
arm_client = actionlib.SimpleActionClient('follow_joint_trajectory',
FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo('...connected.')
# Create a single-point arm trajectory with the arm_goal as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].velocities = [0.0]
arm_trajectory.points[0].accelerations = [0.0]
arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
# Create an empty trajectory goal
arm_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
arm_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal to the action server
arm_client.send_goal(arm_goal)
# Wait for up to 5 seconds for the motion to complete
arm_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo('...done')
def send_traj_(self, trajectory):
print "Sending trajectory"
#prepare goal
trajgoal = FollowJointTrajectoryGoal()
trajgoal.trajectory = trajectory
# send goal
self.joint_spline_trajectory_actionclient_.send_goal(trajgoal)
# wait for result up to 30 seconds
self.joint_spline_trajectory_actionclient_.wait_for_result(timeout=rospy.Duration.from_sec(50))
# analyze result
joint_spline_trajectory_result_ = self.joint_spline_trajectory_actionclient_.get_result()
if self.joint_spline_trajectory_actionclient_.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr("The joint_trajectory action has failed: " + str(joint_spline_trajectory_result_.error_code) )
return -1
else:
rospy.loginfo("The joint_trajectory action has succeeded")
return 0
def redundancy_resolution(q_i_change, q_index, alpha_scale, action_client):
desired_twist = np.array([0, 0, 0, 0, 0, 0])
startingPoint = JointTrajectoryPoint()
startingPoint.positions = arm_group.get_current_joint_values()
startingPoint.time_from_start.secs = 0
q_i_desired = startingPoint.positions[q_index-1] + q_i_change
trajectory = JointTrajectory()
trajectory.points = [startingPoint]
# parameters
N_points = 300
Time_total_sec = 5.0
dt_point2point = Time_total_sec/N_points
time_from_start = 0.0
for i in range(1, N_points):
point = JointTrajectoryPoint()
# compute null space projection
jacobian = arm_group.get_jacobian_matrix(trajectory.points[i-1].positions)
pseudoInverseJacobian = np.linalg.pinv(jacobian) # this mat is 7 by 6
nullSpaceProjector = \
np.identity(7) - pseudoInverseJacobian.dot(jacobian) # this mat is 7 by 7, Equation 5.1, I - J+ J
q_i_current = trajectory.points[i-1].positions[q_index-1]
gradient_q = np.zeros(7)
gradient_q[q_index-1] = q_i_current-q_i_desired
eta = alpha_scale*gradient_q # this vector is 7 by 1, Notes in Asana
jointVelocities = \
pseudoInverseJacobian.dot(desired_twist) \
+ nullSpaceProjector.dot(eta)
# add the desired joint positions to the queue
point.positions = \
(trajectory.points[i-1].positions \
+ (jointVelocities*dt_point2point)).tolist()
time_from_start = time_from_start + dt_point2point
point.time_from_start = rospy.Duration.from_sec(time_from_start)
trajectory.points.append(point)
trajectory.joint_names = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7']
goal = FollowJointTrajectoryGoal()
goal.trajectory = trajectory
action_client.send_goal(goal)
action_client.wait_for_result()
def move_joint(self, angles):
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = ['joint_{}'.format(i+1) for i in range(N_JOINTS)]
point = JointTrajectoryPoint()
point.positions = angles
point.time_from_start = rospy.Duration(3)
goal.trajectory.points.append(point)
print self.jta.send_goal_and_wait(goal)
def go_to(self, collection_of_angles):
joint_points = self._create_joint_trajectory_points(
collection_of_angles)
jt = JointTrajectory(joint_names=self.NAMES, points=joint_points)
tolerance = self.duration + rospy.Duration(2)
goal = FollowJointTrajectoryGoal(trajectory=jt,
goal_time_tolerance=tolerance)
self._send_goal_and_wait_for_result(goal)
def send_grasp_joint_positions(self):
# values in range 0 ... 1
proximal_value = self._widget.proximal_spinbox.value()
distal_value = self._widget.distal_spinbox.value()
# define sets of joints
proximal_joints = [
"sdh_thumb_2_joint", "sdh_finger_12_joint", "sdh_finger_22_joint"
]
distal_joints = [
"sdh_thumb_3_joint", "sdh_finger_13_joint", "sdh_finger_23_joint"
]
static_joints = ["sdh_knuckle_joint"]
all_joints = static_joints + proximal_joints + distal_joints
# map joint ranges from [0..1] to individual set ranges
# proximal range: [-pi/2 .. 0]
# distal range: [0 .. pi/2]
proximal_range = [-math.pi / 2.0, 0.0]
distal_range = [0.0, math.pi / 2.0]
proximal_jpos = proximal_range[0] + proximal_value * (
proximal_range[1] - proximal_range[0])
distal_jpos = distal_range[0] + distal_value * (distal_range[1] -
distal_range[0])
trajectory_goal = FollowJointTrajectoryGoal()
# add single single joint point to trajectory
trajectory_goal.trajectory.points.append(JointTrajectoryPoint())
for jname in all_joints:
trajectory_goal.trajectory.joint_names.append(jname)
# select joint position from set
if jname in static_joints:
trajectory_goal.trajectory.points[0].positions.append(0)
elif jname in proximal_joints:
trajectory_goal.trajectory.points[0].positions.append(
proximal_jpos)
elif jname in distal_joints:
trajectory_goal.trajectory.points[0].positions.append(
distal_jpos)
else:
raise Exception("joint not in set")
# send trajectory and wait for response
self.action_client.send_goal(trajectory_goal)
if self.action_client.wait_for_result(timeout=rospy.Duration(5.0)):
trajectory_result = self.action_client.get_result()
print("set joints to " +
('%s' % trajectory_goal.trajectory.points[0].positions))
self.status_message.setText("set joints")
return trajectory_result.error_code == FollowJointTrajectoryResult.SUCCESSFUL
else:
rospy.logerr("timeout while waiting for response from grasp goal")
self.status_message.setText(
"timeout while waiting for response from grasp goal")
return False
class Elevation_up(py_trees.behaviour.Behaviour):
#class Elevation_up(py_trees_ros.actions.ActionClient):
def __init__(self, name="Elevation_up", target_pose=0.0):
super(Elevation_up, self).__init__(name=name)
self.target_pose = target_pose
self.name = name
self.client = actionlib.SimpleActionClient(
'/arm_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
self.client.wait_for_server()
# self.action_goal.trajectory.header.stamp = rospy.Time.now()
# self.action_goal.goal_time_tolerance = rospy.Duration(30.0)
def setup(self, timeout):
return True
def update(self):
rospy.wait_for_service('/arm_controller/query_state')
try:
query_state = rospy.ServiceProxy('/arm_controller/query_state',
QueryTrajectoryState)
resp = query_state(rospy.Time.now())
except rospy.ServiceException, e:
print "Service call failed: %s" % e
joint_names = resp.name
joint_positions = resp.position
goal = FollowJointTrajectoryGoal()
print(goal)
goal.trajectory.joint_names = list(resp.name)
point = JointTrajectoryPoint()
point.positions = list(resp.position)
# print(point)
# print("point.positions[goal.trajectory.joint_names.index('elevation_joint')] : ", point.positions[goal.trajectory.joint_names.index('elevation_joint')] )
point.positions[goal.trajectory.joint_names.index(
'elevation_joint')] += self.target_pose
if point.positions[goal.trajectory.joint_names.index(
'elevation_joint')] > 0:
point.positions[goal.trajectory.joint_names.index(
'elevation_joint')] = 0
goal.trajectory.points.append(point)
print(goal)
point.time_from_start = rospy.Duration(1.0)
self.client.send_goal(goal)
self.client.wait_for_result()
rospy.sleep(1.0)
self.client.send_goal(goal)
self.client.wait_for_result()
print self.client.get_result()
return py_trees.common.Status.SUCCESS
def _send_joint_trajectory(self,
joints_references,
timeout=rospy.Duration(5),
joint_names=None):
'''
Low level method that sends a array of joint references to the arm.
If timeout is defined, it will wait for timeout*len(joints_reference) seconds for the
completion of the actionlib goal. It will return True as soon as possible when the goal
succeeded. On timeout, it will return False.
'''
if not joints_references:
return
if not joint_names:
if len(joints_references[0]) == len(self.joint_names) + len(
self.torso_joint_names):
joint_names = self.torso_joint_names + self.joint_names
else:
joint_names = self.joint_names
time_from_start = rospy.Duration()
ps = []
for joints_reference in joints_references:
if len(joints_reference) != len(joint_names):
rospy.logwarn(
'Please use the correct %d number of joint references (current = %d'
% (len(joint_names), len(joints_references)))
ps.append(
JointTrajectoryPoint(positions=joints_reference,
time_from_start=time_from_start))
joint_trajectory = JointTrajectory(joint_names=joint_names, points=ps)
goal = FollowJointTrajectoryGoal(trajectory=joint_trajectory,
goal_time_tolerance=timeout)
rospy.logdebug("Send {0} arm to jointcoords \n{1}".format(
self.side, ps))
import time
time.sleep(
0.001
) # This is necessary: the rtt_actionlib in the hardware seems
# to only have a queue size of 1 and runs at 1000 hz. This
# means that if two goals are send approximately at the same
# time (e.g. an arm goal and a torso goal), one of the two
# goals probably won't make it. This sleep makes sure the
# goals will always arrive in different update hooks in the
# hardware TrajectoryActionLib server.
self._ac_joint_traj.send_goal(goal)
if timeout != rospy.Duration(0):
done = self._ac_joint_traj.wait_for_result(timeout *
len(joints_references))
if not done:
rospy.logwarn("Cannot reach joint goal {0}".format(goal))
return done
else:
return None
def __init__(self, server_name):
self.client = actionlib.SimpleActionClient(
server_name, FollowJointTrajectoryAction)
self.joint_positions = []
self.names = ["joint1", "joint2", "joint3", "joint4", "gripper"]
# the list of xyz points we want to plan
xyzrg_positions = [
# [25, -20, 5, 0, 0],
# [25, -15, 5, 0, 0],
# [25, -10, 5, 0, 0],
# [25, -5, 5, 0, 0],
# [25, 0, 5, 0, 0],
# [25, 5, 5, 0, 0],
# [25, 10, 5, 0, 0],
# [25, 15, 5, 0, 0],
# [25, 20, 5, 0, 0]
# [20, -20, 5, 0, 0],
# [20, 20, 5, 0, 0]
# [30, 0, 30, 0, 0],
# [30, 0, 25, 0, 0],
# [30, 0, 20, 0, 0],
# [30, 0, 15, 0, 0],
# [30, 0, 10, 0, 0],
# [30, 0, 5, 0, 0],
# [30, 0, 0, 0, 0]
# [30, 0, 30, 0, -pi/2],
[30, 0, 15, 0, -pi / 2],
[30, 0, 0, 0, -pi / 2],
[30, 0, 0, 0, pi / 4],
[30, 0, 15, 0, pi / 4],
# [30, 0, 0, 0, pi/4],
# [30, 0, 0, 0, -pi/2],
[30, -13, 15, 0, pi / 4],
[30, -13, 5, 0, pi / 4],
[30, -13, 5, 0, -pi / 2],
[30, -13, 15, 0, -pi / 2],
# [0, 0, 55, 0, 0]
]
# initial duration
dur = rospy.Duration(2)
# construct a list of joint positions by calling invkin for each xyz point
for p in xyzrg_positions:
jtp = JointTrajectoryPoint(positions=invkin(p),
velocities=[0.5] * self.N_JOINTS,
time_from_start=dur)
dur += rospy.Duration(2)
self.joint_positions.append(jtp)
self.jt = JointTrajectory(joint_names=self.names,
points=self.joint_positions)
self.goal = FollowJointTrajectoryGoal(trajectory=self.jt,
goal_time_tolerance=dur +
rospy.Duration(2))
def send_trajectory(self,traj,stamp,acceleration=0.5,velocity=0.5,cartesian=False,linear=False):
rate = rospy.Rate(30)
t = rospy.Time(0)
# Make sure that the trajectory 0 is the current joint position
traj.points[0].positions = self.q0
if not self.valid_verify(stamp):
return 'FAILURE - preempted'
if self.simulation:
if not linear:
for pt in traj.points[:-1]:
if not cartesian:
self.send_q(pt.positions,acceleration,velocity)
else:
self.send_cart(pt.positions,acceleration,velocity) ##
self.set_goal(pt.positions)
start_t = rospy.Time.now()
rospy.sleep(rospy.Duration(pt.time_from_start.to_sec() - t.to_sec()))
t = pt.time_from_start
if not cartesian:
self.send_q(traj.points[-1].positions,acceleration,velocity)
else:
self.send_cart(traj.points[-1].positions,acceleration,velocity) ##
self.set_goal(traj.points[-1].positions)
start_t = rospy.Time.now()
# wait until robot is at goal
#while self.moving:
while not self.at_goal:
if (rospy.Time.now() - start_t).to_sec() > 3:
return 'FAILURE - timeout'
rate.sleep()
if self.at_goal:
return 'SUCCESS - moved to pose'
else:
return 'FAILURE - did not reach destination'
else:
self.set_goal(traj.points[-1].positions)
goal = FollowJointTrajectoryGoal(trajectory=traj)
if self.valid_verify(stamp):
self.client.send_goal_and_wait(goal, preempt_timeout=rospy.Duration.from_sec(10.0))
# max time before returning = 30 s
self.client.wait_for_result(rospy.Duration.from_sec(30.0))
res = self.client.get_result()
if res is not None and res.error_code >= 0:
return "SUCCESS"
else:
return "FAILURE - %s"%res.error_code
class Body_Rotate(py_trees.behaviour.Behaviour):
#class Elevation_up(py_trees_ros.actions.ActionClient):
def __init__(self,
name="Body_rotation",
x_offset=0.0,
y_offset=0.0,
z_offset=0.0):
super(Body_Rotate, self).__init__(name=name)
self.x_offset = x_offset
self.y_offset = y_offset
self.name = name
self.client = actionlib.SimpleActionClient(
'/arm_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
self.client.wait_for_server()
def setup(self, timeout):
return True
def update(self):
rospy.wait_for_service('/arm_controller/query_state')
try:
query_state = rospy.ServiceProxy('/arm_controller/query_state',
QueryTrajectoryState)
resp = query_state(rospy.Time.now())
except rospy.ServiceException, e:
print "Service call failed: %s" % e
joint_names = resp.name
joint_positions = resp.position
goal = FollowJointTrajectoryGoal()
print(goal)
goal.trajectory.joint_names = list(resp.name)
point = JointTrajectoryPoint()
point.positions = list(resp.position)
theta = math.atan2(self.y_offset, self.x_offset)
point.positions[goal.trajectory.joint_names.index(
'body_rotate_joint')] = theta
# if point.positions[goal.trajectory.joint_names.index('elevation_joint')] > 0:
# point.positions[goal.trajectory.joint_names.index('elevation_joint')] = 0
goal.trajectory.points.append(point)
print(goal)
point.time_from_start = rospy.Duration(1.0)
self.client.send_goal(goal)
self.client.wait_for_result()
rospy.sleep(1.0)
self.client.send_goal(goal)
self.client.wait_for_result()
print self.client.get_result()
return py_trees.common.Status.SUCCESS
def __init__(self):
rospy.init_node('trajectory_demo')
# Which joints define the arm?
arm_joints = ['x_joint']
# Set the arm back to the resting position
arm_goal = [2]
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
arm_client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo('...connected.')
# Create a single-point arm trajectory with the arm_goal as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].velocities = [0.0]
arm_trajectory.points[0].accelerations = [0.0]
arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
# Create an empty trajectory goal
arm_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
arm_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal to the action server
arm_client.send_goal(arm_goal)
# Wait for up to 5 seconds for the motion to complete
arm_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo('...done')
def __init__(self):
rospy.init_node('trajectory_demo')
# 是否需要回到初始位置
reset = rospy.get_param('~reset', False)
arm_joints = [
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6'
]
# 调整目标位置
if reset:
arm_goal = [0, 0, 0, 0, 0, 0]
else:
arm_goal = [-0.3, -1.0, 0.5, 0.8, 1.0, -0.7]
# 连接机械臂轨迹规划的trajectory action server
rospy.loginfo('waiting for arm trajectory controller...')
arm_client = actionlib.SimpleActionClient(
'arm_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo('...connected.')
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
rospy.loginfo('Moving the arm to goal position...')
arm_goal = FollowJointTrajectoryGoal()
arm_goal.trajectory = arm_trajectory
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
arm_client.send_goal(arm_goal)
arm_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo('...done')
def move1():
goal = FollowJointTrajectoryGoal()
goal.trajectory = JointTrajectory()
goal.trajectory.joint_names = JOINT_NAMES
try:
joint_states = rospy.wait_for_message("joint_states", JointState)
joints_pos = joint_states.position
goal.trajectory.points = [
JointTrajectoryPoint(positions=joints_pos,
velocities=[0] * 6,
time_from_start=rospy.Duration(0.0)),
JointTrajectoryPoint(positions=Q1,
velocities=[0] * 6,
time_from_start=rospy.Duration(3.0)),
]
client.send_goal(goal)
client.wait_for_result()
set_io(0, 1) # 吸气
time.sleep(1)
joint_states = rospy.wait_for_message("joint_states", JointState)
joints_pos = joint_states.position
goal.trajectory.points = [
JointTrajectoryPoint(positions=joints_pos,
velocities=[0] * 6,
time_from_start=rospy.Duration(0.0)),
JointTrajectoryPoint(positions=Q2,
velocities=[0] * 6,
time_from_start=rospy.Duration(3.0)),
JointTrajectoryPoint(positions=Q3,
velocities=[0] * 6,
time_from_start=rospy.Duration(6.0)),
]
client.send_goal(goal)
client.wait_for_result()
set_io(3, 1) # 吹气
time.sleep(1)
set_io(0, 0) # 关闭io
set_io(3, 0)
except KeyboardInterrupt:
client.cancel_goal()
raise
except:
raise
def joints_move(self, angles, duration):
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = self.joint_names
point = JointTrajectoryPoint()
point.positions = angles
point.time_from_start = rospy.Duration(duration)
goal.trajectory.points.append(point)
self.jta.send_goal_and_wait(goal)
rospy.loginfo("Joint move {}".format(angles))
def move_joint(self, angles): # angles类型是元组，实参是所有关节转动的角度
goal = FollowJointTrajectoryGoal()
#joint name should be the same as the tilt.yaml
goal.trajectory.joint_names = ['Joint1', 'Joint2', 'Joint3', 'Joint4']
point = JointTrajectoryPoint()
point.positions = angles # 得到每个关节点的位置
point.time_from_start = rospy.Duration(3)
goal.trajectory.points.append(point)
self.jta.send_goal_and_wait(goal)
def move_joint(self, angles):
goal = FollowJointTrajectoryGoal()
char = self.name[0] #either 'f' or 'b'
goal.trajectory.joint_names = ['motor1','motor2','motor3','motor4','motor5','motor6','motor7','motor8','motor9','motor10','motor11','motor12','motor13','motor14','motor15','motor16','motor17','motor19']
point = JointTrajectoryPoint()
point.positions = angles
point.time_from_start = rospy.Duration(0.5)
goal.trajectory.points.append(point)
self.jta.send_goal_and_wait(goal)
def cb(self, move_base_goal):
move_base_goal = self.tf.transformPose('map',
move_base_goal.target_pose)
goal_js = [
move_base_goal.pose.position.x, move_base_goal.pose.position.y,
rotation_from_matrix(
quaternion_matrix([
move_base_goal.pose.orientation.x,
move_base_goal.pose.orientation.y,
move_base_goal.pose.orientation.z,
move_base_goal.pose.orientation.w
]))[0]
]
current_js = rospy.wait_for_message('whole_body_controller/state',
JointTrajectoryControllerState)
goal = FollowJointTrajectoryGoal()
goal.trajectory.header = move_base_goal.header
goal.trajectory.joint_names = current_js.joint_names
p = JointTrajectoryPoint()
p.time_from_start = rospy.Duration(0.1)
p.positions = current_js.actual.positions
goal.trajectory.points.append(p)
last_p = p
def next_p(goal_p, last_p, f):
diff = (goal_p - last_p)
if diff > 0:
diff = min(diff, self.max_vel * f)
else:
diff = max(diff, -self.max_vel * f)
return last_p + (diff)
while not np.allclose(np.array(goal_js), p.positions[:3]):
p = JointTrajectoryPoint()
p.time_from_start = last_p.time_from_start + rospy.Duration(
1 * self.freq)
p.positions = list(current_js.actual.positions)
p.positions[0] = next_p(goal_js[0], last_p.positions[0], self.freq)
p.positions[1] = next_p(goal_js[1], last_p.positions[1], self.freq)
p.positions[2] = next_p(goal_js[2], last_p.positions[2], self.freq)
goal.trajectory.points.append(p)
last_p = p
self.client.send_goal(goal)
while not self.client.wait_for_result(rospy.Duration(.1)):
if self.server.is_preempt_requested():
rospy.loginfo('new goal, cancel old one')
self.client.cancel_all_goals()
self.server.set_preempted(MoveBaseResult())
break
# result = self.client.get_result()
# if result.error_code == result.SUCCESSFUL:
self.server.set_succeeded(MoveBaseResult())
def __init__(self, arm_name):
"""
Initializing the Parameters and the Client definition to send to the controller
"""
self.goal = FollowJointTrajectoryGoal()
self.client = actionlib.SimpleActionClient(
arm_name + '/joint_trajectory_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
self.client.wait_for_server(rospy.Duration(5))
def move_joint(self, angles):
goal = FollowJointTrajectoryGoal()
char = self.name[0]
goal.trajectory.joint_names = ['head_pan_joint', 'head_tilt_joint']
point = JointTrajectoryPoint()
point.positions = angles
point.time_from_start = rospy.Duration(1)
goal.trajectory.points.append(point)
self.jta.send_goal_and_wait(goal)
def move_joint(self, angles):
goal = FollowJointTrajectoryGoal()
#joint name should be the same as the tilt.yaml
goal.trajectory.joint_names = ['Joint11', 'Joint12','Joint13','Joint14','Joint21', 'Joint22','Joint23','Joint24']
point = JointTrajectoryPoint()
point.positions = angles
point.time_from_start = rospy.Duration(0.5)
goal.trajectory.points.append(point)
self.jta.send_goal_and_wait(goal)
def move_joint(self, angles):
goal = FollowJointTrajectoryGoal()
char = self.name[0] #either 'f' or 'b'
goal.trajectory.joint_names = ['joint_1'+char, 'joint_2'+char,'joint_3'+char]
point = JointTrajectoryPoint()
point.positions = angles
point.time_from_start = rospy.Duration(3)
goal.trajectory.points.append(point)
self.jta.send_goal_and_wait(goal)
def test_fjta_overwrite_goal(self):
# for >= kinetic
if os.environ['ROS_DISTRO'] < 'kinetic':
return True
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
larm = actionlib.SimpleActionClient(
"/larm_controller/follow_joint_trajectory_action",
FollowJointTrajectoryAction)
self.impl_test_jta_overwrite_goal(larm, FollowJointTrajectoryGoal())
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 enviarTrayectoria(self, arm_goal1):
# arm_goal = [0, 0, 0, 0, 0, 0]
# arm_goal = [-0.3, 0.5, -1.0, 1.8, 0.3, 0.6]
arm_joints = ['joint_1',
'joint_2',
'joint_3',
'joint_4',
'joint_5',
'joint_6']
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
rospy.loginfo('...connected.')
# Create a single-point arm trajectory with the arm_goal as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal1
arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(1.5)
rospy.loginfo('Moving the arm to goal position...')
# Create an empty trajectory goal
arm_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
arm_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal to the action server
self.arm_client.send_goal(arm_goal)
# Wait for up to 5 seconds for the motion to complete
self.arm_client.wait_for_result(rospy.Duration(5.0))
self.actualizar_IK(arm_goal1)
rospy.loginfo('...done')
def execute(self, traj_msg):
""" Execute a JointTrajectory message and return a TrajectoryFuture.
@param traj_msg: requested trajectory
@type traj_msg: trajectory_msgs.msg.JointTrajectory
@return future representing the execution of the trajectory
@rtype TrajectoryFuture
"""
from control_msgs.msg import FollowJointTrajectoryGoal
goal_msg = FollowJointTrajectoryGoal()
goal_msg.trajectory = traj_msg
traj_future = TrajectoryFuture(traj_msg)
traj_future._handle = self._client.send_goal(
goal_msg,
transition_cb=traj_future.on_transition,
feedback_cb=traj_future.on_feedback
)
return traj_future
def tuck_arm(self):
while not self.state_recv:
rospy.loginfo("Waiting for controllers to be up...")
rospy.sleep(0.1)
trajectory = JointTrajectory()
trajectory.joint_names = self.joint_names
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = self.tucked
trajectory.points[0].velocities = [0.0 for i in self.joint_names]
trajectory.points[0].accelerations = [0.0 for i in self.joint_names]
trajectory.points[0].time_from_start = rospy.Duration(5.0)
rospy.loginfo("Tucking arm...")
goal = FollowJointTrajectoryGoal()
goal.trajectory = trajectory
goal.goal_time_tolerance = rospy.Duration(0.0)
self.client.send_goal(goal)
self.client.wait_for_result(rospy.Duration(6.0))
rospy.loginfo("...done")
def execute_cb(self, goal):
"""
:type goal: FollowJointTrajectoryGoal
"""
rospy.loginfo("Got a goal!")
# Just resend the goal to the real as
# while checking for cancel goals
# Create goal
g = FollowJointTrajectoryGoal()
# We ignore path_tolerance and goal_tolerance... does not make sense in
# a gripper
g.goal_time_tolerance = goal.goal_time_tolerance
jt = self.substitute_trajectory(goal.trajectory)
g.trajectory = jt
if not self._ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr("Action server for gripper not found!")
self._as.set_aborted(FollowJointTrajectoryResult())
# Send goal subscribing to feedback to republish it
self._ac.send_goal(g, feedback_cb=self._feedback_cb)
# wait for goal to finish while checking if cancel is requested
while not self._ac.wait_for_result(rospy.Duration(0.1)):
rospy.loginfo("Waiting for goal to finish...")
# Deal with goal cancelled
if self._as.is_preempt_requested():
self._ac.cancel_all_goals()
self._as.set_preempted()
return
result = self._ac.get_result()
res = FollowJointTrajectoryResult()
if result:
res.error_code = FollowJointTrajectoryResult.SUCCESSFUL
self._as.set_succeeded(res)
else:
res.error_code = result
self._as.set_aborted(res)
def update(self):
points = []
tick = 0
self.phase = np.random.uniform(np.pi)
while tick <= self.time:
x = 2*np.pi*self.freq*tick + self.phase
positions = [np.sin(x)]*len(self.joint_names)
velocities = [np.cos(x)]*len(self.joint_names)
accelerations = [-np.sin(x)]*len(self.joint_names)
points.append(JointTrajectoryPoint(positions=positions,
velocities=velocities,
accelerations=accelerations,
time_from_start=rospy.Time.from_sec(tick)))
tick += self.step
msg = JointTrajectory(joint_names=self.joint_names, points=points)
msg.header.stamp = rospy.Time.now()
self.pub.publish(msg)
# joint_trajectroy_action
goal = FollowJointTrajectoryGoal()
goal.trajectory = msg
self.client.send_goal(goal)
def move_to_joint(end_pos, duration):
print end_pos
traj = build_traj(get_cur_pos(), end_pos, duration)
# wait for subscribers to connect
# pub = rospy.Publisher('joint_path_command', JointTrajectory)
# if not wait_for_subs(pub, 1, 0.5, 2.0):
# rospy.logwarn('Timeout while waiting for subscribers. Publishing trajectory anyway.')
# pub.publish(traj)
_left_client = actionlib.SimpleActionClient(
'/joint_trajectory_action',
FollowJointTrajectoryAction,
)
print "Waiting..."
_left_client.wait_for_server();
print "Done waiting"
goal = FollowJointTrajectoryGoal()
goal.trajectory = traj
goal.trajectory.header.stamp = rospy.Time.now()
_left_client.send_goal(goal)
print "Sent goal"
rate = rospy.Rate(60) # hz
counter = 0
while counter < duration*70:
rospy.is_shutdown()
rate.sleep()
counter = counter+1
print "Received Result: "
