Skip to content

Instantly share code, notes, and snippets.

@708yamaguchi
Last active November 22, 2021 12:38
Show Gist options
  • Save 708yamaguchi/a331023b0321f01d8fc9f36d7b59273a to your computer and use it in GitHub Desktop.
Save 708yamaguchi/a331023b0321f01d8fc9f36d7b59273a to your computer and use it in GitHub Desktop.
#!/usr/bin/env python
import rospy
from control_msgs.msg import FollowJointTrajectoryActionGoal
from trajectory_msgs.msg import JointTrajectoryPoint
rospy.init_node('dummy_fjt_client')
pub = rospy.Publisher(
"/r_arm_controller/follow_joint_trajectory/goal",
FollowJointTrajectoryActionGoal,
queue_size=1)
def move_pr2(pos, time):
now = rospy.Time.now()
goal = FollowJointTrajectoryActionGoal()
goal.header.stamp = now
goal.goal.trajectory.header.stamp = now
joint_names = [
'r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
goal.goal.trajectory.joint_names = joint_names
point = JointTrajectoryPoint()
point.positions = [pos] * len(joint_names)
point.velocities = [0] * len(joint_names)
# point.accelerations = [0] * len(joint_names)
point.time_from_start = rospy.Time.from_sec(time)
goal.goal.trajectory.points = [point]
pub.publish(goal)
# Do not forget to set pr2 init pose before calling these lines
rospy.sleep(1)
move_pr2(0, 10) # init pose
rospy.sleep(1)
move_pr2(0.1, 10) # move little from init pose
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment