Niko/franka_iml_experiment
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

moveit_interface.cpp update

Browse Source
This commit is contained in:
Niko Feith 2023-09-05 13:35:50 +02:00
parent 0affb75729
commit cc074f4c0a
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
franka_iml_experiment/dmp_node.py
View File

@ -26,7 +26,7 @@ class DMPNode(Node):
self.traj_publisher = self.create_publisher(PoseArray, 'moveit_interface/task_space_trajectory', 10)
def dmp_callback(self, msg):
start = msg.start
start = msg.start_point
self.get_logger().info(f"{start}")
end = np.array(msg.end_point)
time = msg.time
@ -35,7 +35,7 @@ class DMPNode(Node):
# weights = np.vstack((msg.p_x, msg.p_y, msg.p_z, msg.o_x, msg.o_y, msg.o_z, msg.o_w))
weights = np.vstack((msg.p_x, msg.p_y))
dmp = pydmps.dmp_discrete.DMPs_discrete(n_dmps=2, n_bfs=nr_bfs, w=weights, y0=start, goal=end)
dmp = pydmps.dmp_discrete.DMPs_discrete(n_dmps=2, n_bfs=nr_bfs, w=weights, y0=start[:2], goal=end[:2])
y_track, _, _ = dmp.rollout(tau=time)
pose_msg = PoseArray()