def sendJointCommandSim(self, joint_msg):
joint_name_lst = [
'front_right_leg3_joint', 'front_right_leg2_joint',
'front_right_leg1_joint', 'front_left_leg3_joint',
'front_left_leg2_joint', 'front_left_leg1_joint',
'back_right_leg3_joint', 'back_right_leg2_joint',
'back_right_leg1_joint', 'back_left_leg3_joint',
'back_left_leg2_joint', 'back_left_leg1_joint'
]
jtp_msg = JointTrajectory()
jtp_msg.joint_names = joint_name_lst
point = JointTrajectoryPoint()
point.positions = self.__makeJointStateMsg2(joint_msg)
point.velocities = np.zeros(len(joint_name_lst))
point.accelerations = np.zeros(len(joint_name_lst))
point.effort = np.zeros(len(joint_name_lst))
point.time_from_start = rospy.Duration(1.0 / 60.0)
jtp_msg.points.append(point)
self.jtp.publish(jtp_msg)
def publish_arm(lift, flex, roll, wrist_flex, wrist_roll):
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [latest_positions[name] for name in traj.joint_names]
current_positions[0] = lift
current_positions[1] = flex
current_positions[2] = roll
current_positions[3] = wrist_flex
current_positions[4] = wrist_roll
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
def talker():
rospy.init_node('joint_trajectory_publisher', anonymous=True)
pub = rospy.Publisher('arm_controller/command', JointTrajectory, queue_size=10)
#pub = rospy.Publisher('/joint_group_position_controller/command', Float64MultiArray, queue_size=10)
rospy.sleep(0.5)
#tes = Float64MultiArray()
msg = JointTrajectory()
msg.header.stamp = rospy.Time.now()
msg.joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ]
msg.points = [JointTrajectoryPoint() for i in range(1)]
msg.points[0].positions = [0, 0, 0, 0, 0, 0]
msg.points[0].time_from_start = rospy.Time(1.0)
#tes.data = [math.pi/2, -math.pi/6.0, -math.pi/2, -math.pi/3, -math.pi/2, 0.0]
#tes.data = [-math.pi/2, -2.8 * math.pi/6, -math.pi/3, -1*math.pi/1.5, math.pi/2, -math.pi/2];
pub.publish(msg)
rospy.sleep(0.5)
def get_trajectory_message(self, action, robot_id=0):
"""
Helper function.
Wraps an action vector of joint angles into a JointTrajectory message.
The velocities, accelerations, and effort do not control the arm motion
"""
# Set up a trajectory message to publish.
action_msg = JointTrajectory()
action_msg.joint_names = self.environment['joint_order']
# Create a point to tell the robot to move to.
target = JointTrajectoryPoint()
action_float = [float(i) for i in action]
target.positions = action_float
# These times determine the speed at which the robot moves:
# it tries to reach the specified target position in 'slowness' time.
target.time_from_start.secs = self.environment['slowness']
# target.time_from_start.nsecs = self.environment['slowness']
# Package the single point into a trajectory of points with length 1.
action_msg.points = [target]
return action_msg
def up():
rospy.init_node('demo_controller')
cmd_publisher = rospy.Publisher('/arm_controller/command',
JointTrajectory,
queue_size=10)
cmd = JointTrajectory()
cmd.header.stamp = rospy.Time.now()
cmd.joint_names = joint_names
time.sleep(1)
for i in range(len(move)):
points = []
positions = move[i]
point = JointTrajectoryPoint()
point.positions = positions
point.time_from_start.secs = 0.0
point.time_from_start.nsecs = 157114594
points.append(point)
cmd.points = points
cmd_publisher.publish(cmd)
print 'Sent message!'
def move_joints(self, pos):
self.check_joints_connection()
rospy.wait_for_service("/gazebo/clear_joint_forces")
for name in self.joint_name_list:
self.clear_forces(joint_name=name)
# self.clear_forces(joint_name=self.joint_name_string)
jtp_msg = JointTrajectory()
jtp_msg.joint_names = self.joint_name_list
point = JointTrajectoryPoint()
point.positions = pos
# See urdf for vel (FEETECH FS-90 Servo)
point.velocities = self.jtp_zeros # np.ones(18) * 8.0
# No Acc Data
point.accelerations = self.jtp_zeros
# See urdf for effort (FEETECH FS-90 Servo)
point.effort = np.ones(18) * 0.15
# one ns, the minimum time
point.time_from_start = rospy.Duration(1e-9)
jtp_msg.points.append(point)
self.jtp.publish(jtp_msg)
def parseXapPoses(self, xaplibrary):
try:
poses = xapparser.getpostures(xaplibrary)
except RuntimeError as re:
rospy.logwarn("Error while parsing the XAP file: %s" % str(re))
return
for name, pose in poses.items():
trajectory = JointTrajectory()
trajectory.joint_names = pose.keys()
joint_values = pose.values()
point = JointTrajectoryPoint()
point.time_from_start = Duration(2.0) # hardcoded duration!
point.positions = pose.values()
trajectory.points = [point]
self.poseLibrary[name] = trajectory
def moveJoint(self, jointcmds, nbJoints):
topic_name = '/j2n6s300/effort_joint_trajectory_controller/command'
pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1)
jointCmd = JointTrajectory()
point = JointTrajectoryPoint()
jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0)
point.time_from_start = rospy.Duration.from_sec(5.0)
for i in range(0, nbJoints):
jointCmd.joint_names.append('j2n6s300_joint_' + str(i + 1))
point.positions.append(jointcmds[i])
point.velocities.append(0)
point.accelerations.append(0)
point.effort.append(0)
jointCmd.points.append(point)
rate = rospy.Rate(100)
count = 0
while count < 500:
pub.publish(jointCmd)
count = count + 1
rate.sleep()
def execute_pose(pose, planning_time=1):
positions = solve_ik(pose)
if positions != None:
# simulation
jt = JointTrajectory()
jt.joint_names = joint_names
jt.header.stamp = rospy.Time.now()
jtp = JointTrajectoryPoint()
jtp.positions = positions
jtp.velocities = [0.5] * 6
jtp.time_from_start = rospy.Duration(planning_time)
jt.points.append(jtp)
sim_pub.publish(jt)
# real
builder.set_joint_goal(joint_names, positions)
builder.allowed_planning_time = planning_time
builder.plan_only = False
to_send = builder.build()
action_client.send_goal(to_send)
action_client.wait_for_result()
def send_joint_position_actionlib(settings):
# rosnode の初期化
rospy.init_node('send_joint_position')
# トピック名，メッセージ型を使って ActionLib client を定義
client = actionlib.SimpleActionClient(
'/fullbody_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
client.wait_for_server() # ActionLib のサーバと通信が接続されることを確認
# ActionLib client の goal を指定
# http://wiki.ros.org/actionlib_tutorials/Tutorials の
# Writing a Simple Action Client (Python) を参照
# __TOPIC_PREFIX__Action で actionlib.SimpleActionClient を初期化
# ゴールオブジェクトは __TOPIC_PREFIX__Goal を使って生成
goal = FollowJointTrajectoryGoal()
goal.trajectory = JointTrajectory()
goal.trajectory.header.stamp = rospy.Time.now()
goal.trajectory.joint_names = [
'arm_joint1', 'arm_joint2', 'arm_joint3', 'arm_joint4', 'arm_joint5',
'arm_joint6'
]
for i in range(5):
point = JointTrajectoryPoint()
point.positions = [
math.pi / 2, 0, math.pi / 4 * (i % 2), 0, math.pi / 2, math.pi / 2
]
point.time_from_start = rospy.Duration(1.0 + i)
goal.trajectory.points.append(point)
client.send_goal(goal)
rospy.loginfo("wait for goal ...")
while (1):
key = getKey()
if (client.get_state() == 3):
rospy.loginfo("done")
break
else:
if (key == 'c'):
client.cancel_goal()
rospy.loginfo("canceling")
break
client.wait_for_result() # ロボットの動作が終わるまで待つ．
def gripper_send_position_goal(self,
position=0.3,
velocity=0.4,
action='move'):
"""Send position goal to the gripper
Keyword Arguments:
position {float} -- Gripper angle (default: {0.3})
velocity {float} -- Gripper velocity profile (default: {0.4})
action {str} -- Gripper movement (default: {'move'})
"""
self.turn_gripper_position_controller_on()
duration = 0.2
if action == 'pre_grasp_angle':
gripper_angle_grasp = deepcopy(self.d[-2])
max_distance = 0.085
angular_coeff = 0.11
K = 1 # lower numbers = higher angles
angle = (max_distance - gripper_angle_grasp) / angular_coeff * K
position = angle
velocity = 0.4
elif action == 'pick':
position = 0.75
velocity = 0.05
duration = 8.0
goal = FollowJointTrajectoryGoal()
goal.trajectory = JointTrajectory()
goal.trajectory.joint_names = self.robotiq_joint_name
goal.trajectory.points.append(
JointTrajectoryPoint(positions=[position],
velocities=[velocity],
accelerations=[0.0],
time_from_start=rospy.Duration(duration)))
self.client_gripper.send_goal(goal)
if action == 'pick':
while not rospy.is_shutdown(
) and not self.left_collision and not self.right_collision:
pass
self.client_gripper.cancel_goal()
def OARbot_nav():
global velocity, bot, goal,x,y
x=0
y=0
rospy.init_node('OARbot_control', anonymous=True)
rate=rospy.Rate(100)
goal=location(x,y)
bot=rospy.Publisher('/cmd_vel', Twist, queue_size=10)
arm=rospy.Publisher('/j2n6s300/effort_joint_trajectory_controller/command',JointTrajectory, queue_size=1)
rospy.Subscriber('/odom',Odometry, pose_callback)
velocity=Twist()
jointcmds=[0.0,2.9,1.3,4.2,1.4,0.0]
jointCmd=JointTrajectory()
point=JointTrajectoryPoint()
jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0);
point.time_from_start = rospy.Duration.from_sec(5.0)
for i in range(0, 6):
jointCmd.joint_names.append('j2n6s300_joint_'+str(i+1))
point.positions.append(jointcmds[i])
point.velocities.append(5)
point.accelerations.append(0)
point.effort.append(0)
jointCmd.points.append(point)
rospy.sleep(2)
key=raw_input("ready?")
x=float(raw_input("Where to?"))
y=float(raw_input())
goal=location(x,y)
walk_thread=threading.Thread(target=walk, args=())
walk_thread.start()
talk_thread=threading.Thread(target=talk, args=())
talk_thread.start()
rospy.sleep(1)
while not rospy.is_shutdown():
x=float(raw_input("Where to?"))
y=float(raw_input())
goal=location(x,y)
rospy.sleep(1)
def move(self, *args):
try:
# Frame id and joints
frame_id = "base"
arm_joints = [
'base_rotate', 'shoulder_tilt', 'elbow_tilt', 'wrist_tilt',
'wrist_rotate', 'open_gripper'
]
pos = Pose()
pos.position.x = float(self.x.get())
pos.position.y = float(self.y.get())
pos.position.z = float(self.z.get())
pitch = float(self.pitch.get())
roll = float(self.roll.get())
joint_pos = [0, 0, 0, 0, 0, 0]
response = self.IkService(pos, pitch, roll)
joint_pos[0] = response.j1
joint_pos[1] = response.j2
joint_pos[2] = response.j3
joint_pos[3] = response.j4
joint_pos[4] = response.j5
joint_pos[5] = float(self.gripper.get())
# Create a single-point arm trajectory with the joint_pos as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = joint_pos
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(2.0)
arm_trajectory.header.frame_id = frame_id
arm_trajectory.header.stamp = rospy.Time.now()
# Publish trajectry
rospy.sleep(0.5)
self.command.publish(arm_trajectory)
except ValueError:
pass
def moveToJointPosition(self, req):
jointStateFinal = req.joint_state
jointStateStart = self.getRobotState()
rospy.loginfo("moving robot to joint position %s",
str(jointStateFinal.position))
# figure out the duration based on max joint degrees per second
print "jointStateStart \n", jointStateStart
print "jointStateFinal \n", jointStateFinal
print "jointStateStart.position = ", jointStateStart.position
q_start = np.array(jointStateStart.position)
q_end = np.array(jointStateFinal.position)
minPlanTime = 1.0
maxDeltaRadians = np.max(np.abs(q_end - q_start))
duration = maxDeltaRadians / np.deg2rad(
req.max_joint_degrees_per_second)
duration = max(duration, minPlanTime)
rospy.loginfo("rescaled plan duration is %.2f seconds", duration)
trajectory = JointTrajectory()
trajectory.header.stamp = rospy.Time.now()
startTime = rospy.Duration.from_sec(0)
endTime = rospy.Duration.from_sec(duration)
startPoint = RobotMovementService.jointTrajectoryPointFromJointState(
jointStateStart, startTime)
endPoint = RobotMovementService.jointTrajectoryPointFromJointState(
jointStateFinal, endTime)
trajectory.points = [startPoint, endPoint]
trajectory.joint_names = self.jointNames
trajectoryServiceResponse = self.trajectoryService(trajectory)
resp = robot_msgs.srv.MoveToJointPositionResponse(
trajectoryServiceResponse.success)
return resp
def test():
pubJoint = rospy.Publisher('/typea/joint_trajectory_controller/command', JointTrajectory, queue_size=10)
rospy.init_node('bioloid_command_test', anonymous=True)
rate = rospy.Rate(20) # 2hz
rospy.loginfo("\33[96mBioloid Command Test\33[0m")
joint = JointTrajectory()
points = JointTrajectoryPoint()
nameJoints = ['l_shoulder_swing_joint', 'l_shoulder_lateral_joint', 'l_elbow_joint',
'r_shoulder_swing_joint','r_shoulder_lateral_joint', 'r_elbow_joint',
'l_hip_twist_joint', 'l_hip_lateral_joint','l_hip_swing_joint',
'l_knee_joint', 'l_ankle_swing_joint', 'l_ankle_lateral_joint',
'r_hip_twist_joint','r_hip_lateral_joint', 'r_hip_swing_joint',
'r_knee_joint', 'r_ankle_swing_joint', 'r_ankle_lateral_joint']
joint.joint_names = nameJoints
_actionArmDown = [2,-2,1,2,2,1,0,-0.4,0,0,0,0,0,0,0,0,0,0]
_actionArmUp = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
arm_state = "Up"
rospy.loginfo("\33[96mStarting Arm Up\33[0m")
switch_timer = rospy.Time.now()
while not rospy.is_shutdown():
if rospy.Time.now() - switch_timer > rospy.Duration(2):
switch_timer = rospy.Time.now()
if arm_state == "Up":
arm_state = "Down"
rospy.loginfo("\33[96mArm Down\33[0m")
elif arm_state == "Down":
arm_state = "Up"
rospy.loginfo("\33[96mArm Up\33[0m")
if arm_state == "Down":
points.positions = _actionArmDown
elif arm_state == "Up":
points.positions = _actionArmUp
points.time_from_start = rospy.Duration(0.01, 0)
joint.points = [points]
joint.header.stamp = rospy.Time.now()
#print joint
pubJoint.publish(joint)
# Time Sync
rate.sleep()
def trayectoria_definitiva2():
pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=10)
rospy.init_node('trayectoria_definitiva2', anonymous=True)
trajectory = JointTrajectory()
trajectory.joint_names = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "gripper_revolute_joint"]
trajectory.header.stamp = rospy.Time.now()
trajectory.header.frame_id = "base_footprint";
#Pulled back pose
jtp = JointTrajectoryPoint()
jtp.positions = [0.0, -0.951, 0.434, 1.502, 0.0, 1.7]
jtp.time_from_start = rospy.Duration(3)
trajectory.points.append(jtp)
#Aproximacion inicial
jtp_2 = JointTrajectoryPoint()
jtp_2.positions = [0.0, -0.937, 0.072, 0.853, 0.0, 1.7]
jtp_2.time_from_start = rospy.Duration(6)
trajectory.points.append(jtp_2)
#Aproximacion a la pieza
jtp_3 = JointTrajectoryPoint()
jtp_3.positions = [0.0, -0.383, -0.2531, 0.638, 0.0, 1.7]
jtp_3.time_from_start = rospy.Duration(9)
trajectory.points.append(jtp_3)
#Cierra la pinza
jtp_4 = JointTrajectoryPoint()
jtp_4.positions = [0.0, -0.383, -0.2531, 0.638, 0.0, 0.0]
jtp_4.time_from_start = rospy.Duration(12)
trajectory.points.append(jtp_4)
#Retroceso
jtp_5 = JointTrajectoryPoint()
jtp_5.positions = [0.0, -0.937, 0.072, 0.853, 0.0, 0.0]
jtp_5.time_from_start = rospy.Duration(15)
trajectory.points.append(jtp_5)
#Pulled back pose
jtp_6 = JointTrajectoryPoint()
jtp_6.positions = [0.0, -0.951, 0.434, 1.502, 0.0, 0.0]
jtp_6.time_from_start = rospy.Duration(18)
trajectory.points.append(jtp_6)
pub.publish(trajectory)
def __create_joint_trajectory(self, joint_names, positions, velocities,
accelerations, duration):
"""Creating JointTrajectory message.
@joint_names : joint_name
@positions : [radian]
@velocities : [radian/sec]
@accelerations: [radian/sec^2]
@duration : [sec]
"""
msg = JointTrajectory()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = ''
msg.joint_names = joint_names
point = JointTrajectoryPoint()
point.positions = positions
point.velocities = velocities
point.accelerations = accelerations
point.effort = [0.0 for _ in range(len(joint_names))]
point.time_from_start = rospy.Duration(secs=duration, nsecs=0)
msg.points.append(point)
return msg
def move_ur5e_joint_trajectory():
jtpt = JointTrajectoryPoint()
jt_ur5e = JointTrajectory()
#while not rospy.is_shutdown():
jt_ur5e.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
jtpt.positions = [1.57, 0, 0, 0, 0, 0]
jtpt.velocities = [1, 1, 1, 1, 1, 1]
jtpt.accelerations = [1, 1, 1, 1, 1, 1]
jtpt.time_from_start = 2
jt_ur5e.points.append(jtpt)
jt_pub_ur5e.publish(jt_ur5e)
#rospy.loginfo(jtpt)
rospy.loginfo(jt_ur5e)
rospy.sleep(1)
def _update_cmd_cb(self):
dur = []
traj = JointTrajectory()
traj.joint_names = self._joint_names
point = JointTrajectoryPoint()
for name in traj.joint_names:
pos = self._joint_pos[name]['position']
cmd = pos
try:
cmd = self._joint_pos[name]['command']
except (KeyError):
pass
max_vel = self._robot_joint_limits[name]['max_velocity']
dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur))
point.positions.append(cmd)
point.time_from_start = Duration(seconds=(max(dur) /
self._speed_scale)).to_msg()
traj.points.append(point)
self._cmd_pub.publish(traj)
def _flexible_trajectory(self, position, time=5.0, vel=None):
""" Publish point by point making it more flexible for real-time control """
# Set up a trajectory message to publish.
action_msg = JointTrajectory()
action_msg.joint_names = JOINT_ORDER
# Create a point to tell the robot to move to.
target = JointTrajectoryPoint()
target.positions = position
# These times determine the speed at which the robot moves:
# it tries to reach the specified target position in 'slowness' time.
if vel is not None:
target.velocities = [vel] * 6
target.time_from_start = rospy.Duration(time)
# Package the single point into a trajectory of points with length 1.
action_msg.points = [target]
self._flex_trajectory_pub.publish(action_msg)
def _get_ur_trajectory_message(self, action, slowness):
"""Helper function only called by reset() and run_trial().
Wraps an action vector of joint angles into a JointTrajectory message.
The velocities, accelerations, and effort do not control the arm motion"""
# Set up a trajectory message to publish.
action_msg = JointTrajectory()
action_msg.joint_names = self._hyperparams['joint_order']
# Create a point to tell the robot to move to.
target = JointTrajectoryPoint()
target.positions = action
# These times determine the speed at which the robot moves:
# it tries to reach the specified target position in 'slowness' time.
target.time_from_start = rospy.Duration(slowness)
# Package the single point into a trajectory of points with length 1.
action_msg.points = [target]
return action_msg
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 moveJoint(jointcmds, prefix, nbJoints):
topic_name = '/' + prefix + '/arm_controller/command'
print("topic_name: ", topic_name)
pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1)
jointCmd = JointTrajectory()
point = JointTrajectoryPoint()
jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0)
point.time_from_start = rospy.Duration.from_sec(0.)
for i in range(0, nbJoints):
jointCmd.joint_names.append('drive' + str(i + 1) + '_joint')
point.positions.append(jointcmds[i])
point.velocities.append(0)
point.accelerations.append(0)
point.effort.append(0)
jointCmd.points.append(point)
rate = rospy.Rate(1)
count = 0
while (count < 2):
pub.publish(jointCmd)
count += 1
rate.sleep()
def init_trajectory(self):
"""Initialize a new target trajectory."""
state = self.jointStatePublisher.last_joint_states
self.trajectory_t0 = self.robot.getTime()
self.trajectory = self.gripper_trajectory = JointTrajectory()
self.trajectory.joint_names = self.prefixedJointNames
self.gripper_trajectory_names = self.gripper
self.trajectory.points = [
JointTrajectoryPoint(positions=state.position if state else [0] *
7,
velocities=[0] * 7,
accelerations=[0] * 7,
time_from_start=rospy.Duration(0.0))
]
self.gripper_trajectory.points = [
JointTrajectoryPoint(positions=state.position if state else [0] *
2,
velocities=[0] * 2,
accelerations=[0] * 2,
time_from_start=rospy.Duration(0.0))
]
def make_JointTrajectory_from_joints(vals=[], names=[]):
if not len(vals) == len(names):
print("ERROR: length miss match")
return None
JT_msg = JointTrajectory()
JT_msg.joint_names = names
JTP_msg = JointTrajectoryPoint()
JT_msg.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0)
JTP_msg.time_from_start = rospy.Duration.from_sec(5.0)
# JTP_msg.time_from_start = 0.0
JTP_msg.positions = vals
JTP_msg.velocities = [0.0] * len(names)
JTP_msg.accelerations = [0.0] * len(names)
JTP_msg.effort = [0.0] * len(names)
JT_msg.points.append(JTP_msg)
return JT_msg
def move_joints(self, joints_array):
vel_cmd = JointTrajectory()
vel_cmd.joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
vel_cmd.header.stamp = rospy.Time.now()
# create a JTP instance and configure it
jtp = JointTrajectoryPoint(positions=[0] * 6,
velocities=[0] * 6,
time_from_start=rospy.Duration(.0))
# jtp.velocities = [joints_array[0], joints_array[1], joints_array[2], joints_array[3], joints_array[4], joints_array[5] ]
jtp.positions = [
joints_array[0], joints_array[1], joints_array[2], joints_array[3],
joints_array[4], joints_array[5]
]
# print("jtp", jtp)
# setup the reset of the pt
vel_cmd.points = [jtp]
self._joint_traj_pub.publish(vel_cmd)
def __init__(self, arm_prefix):
rospy.init_node('dmp_fitter', anonymous=True)
rospy.Subscriber('joint_states', JointState, self.callback)
self.joint_names = [
arm_prefix + joint for joint in [
"_shoulder_pan_joint", "_shoulder_lift_joint",
"_upper_arm_roll_joint", "_elbow_flex_joint",
"_forearm_roll_joint", "_wrist_flex_joint", "_wrist_roll_joint"
]
]
self.js_0 = None
self.js_1 = None
self.jt = JointTrajectory()
self.traj_pub = rospy.Publisher('motion_segment',
JointTrajectory,
queue_size=1)
self.moving = False
self.traj = []
self.dmp_count = 0
def move_sawyer_joint_trajectory(xopt, duration=0.1):
'''
generally for use with gazebo
:param xopt:
:return:
'''
global jt_pub_sawyer
jt_sawyer = JointTrajectory()
jt_sawyer.joint_names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5',
'right_j6'
]
jtpt = JointTrajectoryPoint()
jtpt.positions = [
xopt[0], xopt[1], xopt[2], xopt[3], xopt[4], xopt[5], xopt[6]
]
jtpt.time_from_start = rospy.Duration.from_sec(duration)
jt_sawyer.points.append(jtpt)
jt_pub_sawyer.publish(jt_sawyer)
def substitute_trajectory(self, trajectory):
jt = JointTrajectory()
jt.joint_names = self.real_joint_names
jt.header = trajectory.header
for jtp_in in trajectory.points:
jtp = JointTrajectoryPoint()
if jtp_in.positions:
joint_position = jtp_in.positions[0]
# Each finger will go to half of the size
jtp.positions = [joint_position / 2.0, joint_position / 2.0]
jtp.time_from_start = jtp_in.time_from_start
else:
rospy.logwarn("Trajectory has no positions...")
if jtp_in.velocities:
jtp.velocities.append(jtp_in.velocities[0])
if jtp_in.accelerations:
jtp.accelerations.append(jtp_in.accelerations[0])
if jtp_in.effort:
jtp.effort.append(jtp_in.effort[0])
jt.points.append(jtp)
return jt
def moveFingers (jointcmds,prefix,nbJoints):
# topic_name = '/' + prefix + '/effort_finger_trajectory_controller/command'
topic_name = '/' + prefix + '/position_finger_trajectory_controller/command'
pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1)
jointCmd = JointTrajectory()
point = JointTrajectoryPoint()
jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0);
point.time_from_start = rospy.Duration.from_sec(5.0)
for i in range(0, nbJoints):
jointCmd.joint_names.append(prefix +'_joint_finger_'+str(i+1))
point.positions.append(jointcmds[i])
point.velocities.append(0)
point.accelerations.append(0)
point.effort.append(0)
jointCmd.points.append(point)
rate = rospy.Rate(100)
count = 0
while (count < 50):
pub.publish(jointCmd)
count = count + 1
rate.sleep()
