skrobot.planner.utils.forward_kinematics_multi

skrobot.planner.utils.forward_kinematics_multi(robot_model, joint_list, av, move_target_list, with_rot, with_base, with_jacobian)[source]

Compute fk for multiple feature points

Parameters:

  • robot_model (skrobot.model.CascadedLink) – robot model.

  • joint_list (list[skrobot.model.Joint]) – joint to be controlled

  • av (numpy.ndarray(n_dof,)) – angle vector.

  • move_target_list (list[skrobot.coordinates.CascadedCoords]) – the list has n_feature elements. Each element is the coordinate of the features points.

  • with_rot (bool) – If set to True, 7(3 + 4) dim pose-fk is also computed. Otherwise, 3 dim point-fk is computed.

  • with_base (bool) – If with_base=False, n_dof is the number of joints n_joint, but if with_base=True, n_dof = n_joint + 3.

Returns:

  • pose_arr (numpy.ndarray(n_feature, dim_pose)) – array of pose of each feature points. dim_pose=7’ if `with_rot=True. Otherwise, dim_pose=3.

  • jac_arr (numpy.ndarray(n_feature, dim_pose, n_dof)) – array of jacobian of each feature points.